爪車

ヒト型ロボット 50 運動学に取り組み中

ロボットの運動学のプログラムに取り組んでいます。
いろいろと実験的なことをしています。成果は上々です。

Screenshot-opengl-20100226.png

ヤコビアンは偉大なり。

ヒト型ロボット 49 プログラム再構築中

OSの再インストールの際に、バックアップをミスって消失してしまったプログラムを再び開発し直しています。

OpenGLを用いた三次元プロット用のプログラムを再構築。
以前のものよりも機能強化。アンチエイリアスをサポート。

Screenshot-opengl-20100223.png
点と線でロボットモデルの描写。なんかキモイ・・・。

平面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

ヒト型ロボット 48 下半身完成

下半身が完成しました。

前のものより脚の長さを4cm縮めて脚長(脚の付け根のピッチ軸から足部のピッチ軸まで)が40cmになりました。
サーボはすべてKRS-4014SHVです。

加工精度が悪く、何箇所かネジが締まりませんでした。



今回のロボットでは、脚の付け根のヨー軸を太ももに持ってきてみました。
腰周りがスッキリしていい感じです。
組み立てた後で気が付いたのですが、この構造だと脚を90°前方に振り上げたときに、自由度が縮退して横には脚が開かなくなりました。
まあ、しゃがんだ状態での横歩きなどをしないのであれば問題ないと思います。
通常のサーボのような回り対偶を3つ組み合わせての機構だと、どうしてもどこかで自由度の縮退が起こってしまいます。完全な球関節の代わりにはならないですね。

膝は二重間接にしてみました。直列ダブルサーボです。
これで綺麗な正座ができる、と思ったのですが、サーボのトルク不足で一度しゃがんだら自力では立てなくなると思います。
ダブルサーボといっても直列なのでトルクが上がるわけではありません。
間接の速度が2倍になりますが、そんなに速くなっても歩行動作にはあまりメリットがないような気がします。
ガタが増えてしまうだけのような気も・・・動かしてみるまではどうなるかはわからないですね。

今回はけっこう無理して脚長を長くしています。KRS-4014SHVでもかなり限界だと思います。
トルク不足やギヤの破損などが心配です。サーボが壊れなければ良いのですが。
前回のロボットで脚長30cmでの歩行に成功しているので、膝をなるべく曲げないようにして、サーボへの負担をなるべく減らすようにすれば、今回の脚長でも歩行できるかなと思っています。

ヒト型ロボット 47 途中経過

新型制作の途中経過です。

部品の加工があらかた終わり、組み立ててみたのですが・・・
なんだかバランスが悪い・・・脚が長すぎる気が・・・。

DSCN1256r.jpg

縮めてみます。

ヒト型ロボット 46 新型開発開始

プログラム開発はしばらくやる気が起きそうにないので、新しくロボットを作ることにしました。

より人間らしさを求め、更なる大型化に挑戦してみます。
前回のロボットよりも10cmくらい脚の長さを延ばしてみる予定です。
脚の構造も変えてみようかと思います。


新型制作のため前回のロボットは解体しました。その部品の一部。
ICS2.0制御用のIDがわからなくならないように、シールで番号を付けてみました。

大変なことに・・・

手違いで、ここ数ヶ月間に開発してきたプログラムを消去してしまいました。

都合によりlinuxを再インストールしたのですが、その際に全部消去してしまいました。

再インストールの前にバックアップをとっていたと思っていたのですが、どうやらバックアップをとるディレクトリを間違えていたようで・・・。

以前とったバックアップが他のパソコンにあったのですが、ファイルの最終更新日が12月6日・・・。

あぁ・・・

ヒト型ロボット 45 3次元に拡張

つま先を考慮した2次元線形倒立振子モデルに左右方向も考慮して、3次元に拡張してみました。

支持脚の切替は、前後方向は着地タイミングによる調整、左右方向は着地位置の調整による制御です。

つま先つきの3次元線形倒立振子モデルだと、支持脚切替が、踵とつま先部で起こることになります。
しかし、左右方向の着地位置調整に踵とつま先で2回ともすると、踵とつま先の距離を一定に保つのが難しくなってしまうので、つま先の着地位置は踵の着地位置に従属とし、踵の位置が決まればつま先の位置も決まるようにして、着地点の調整は踵が接地するときのみにしました。
ただ、この方法では、つま先での着地位置の調整による運動の制御ができないので、一歩の最終状態を目標最終状態に近づけることは困難になります。
一歩の最終状態を目標に近づけるためには、踵の着地位置調整時につま先のことも考慮する必要があります。

つま先の着地位置が踵の着地位置に従属で、支持脚切替タイミングが前後方向の運動によって事前に決定するので、一歩の最終状態は踵の着地位置により一意に決まります。
この踵の着地位置による最終状態と、目標最終状態の誤差を評価することで、修正された踵の着地位置を求めることができます。

踵の修正着地位置を求める計算は面倒なことになりそうだったので、フリーの数式処理ソフトMaximaで処理しました。(WindowsでwxMaxima利用)

Maximaは文字式をそのまま、微分積分を含めた各種計算、式の展開などが簡単にでき、非常に便利なソフトです。

参考までに踵の修正着地位置はこうなりました。
spx=-((a*Sf*Sm+a*Cf*Cm-a)*Tc^2*xd+
((-a*Sf^2*Sm^2+(a-2*a*Cf*Cm)*Sf*Sm-a*Cf^2*Cm^2+a*Cf*Cm)*Tc^2-b*Cf^2*Sm^2-2*b*Cf*Cm*Sf*Sm-b*Cm^2*Sf^2)*x1+
(-a*Cf*dx1*Sf*Sm^2+((a*Cf-a*Cf^2*Cm)*dx1-a*Cm*dx1*Sf^2)*Sm+(a*Cm-a*Cf*Cm^2)*dx1*Sf)*Tc^3+
((a*Cm-a)*d*Sf*Sm+(a*Cf*Cm^2+(-a*Cf-a)*Cm+a)*d)*Tc^2+
(-b*Cf*dx1*Sf*Sm^2+(-b*Cm*dx1*Sf^2+b*Cf*dxd-b*Cf^2*Cm*dx1)*Sm+(b*Cm*dxd-b*Cf*Cm^2*dx1)*Sf)*Tc+b*Cf*
d*Sm^2+b*Cm*d*Sf*Sm)/((a*Sf^2*Sm^2+(2*a*Cf*Cm-2*a)*Sf*Sm+a*Cf^2*Cm^2-2*a*Cf*Cm+a)*Tc^2+b*Cf^2*Sm^2+2*b*
Cf*Cm*Sf*Sm+b*Cm^2*Sf^2)


Maximaには式を簡単化する機能があるのですが、あまりにも式が複雑だと処理できないみたいです。
手作業で式をまとめるのは面倒だったので、このまま利用しています。

Cでプログラムして、gnuplotで出力。

Screenshot-Gnuplot-20100203.gif
赤線部が踵による支持、緑線部がつま先による支持。

なんとかうまくいきそうです。

ヒト型ロボット 44 重心の上下運動を考慮した2次元線形倒立振子モデル

通常の2次元線形倒立振子は、重心の鉛直方向の運動がなく、一定の高さを保ったまま水平方向に運動しますが、
重心が適当な傾きの拘束直線上を運動するように制御すれば、水平方向の運動はそのままで、重心を上下に運動させることがができます。

この方法を利用して、つま先を考慮した2次元線形倒立振子モデルに、重心の鉛直方向の運動を取り入れてみました。

2d_lip_20100201.gif
水平方向の運動は、前回のつま先を考慮した2次元線形倒立振子モデルと同一です。
複数の拘束直線でつないでいます。

倒れこむような運動ができるので、脚長の変化を少なくすることができ、
ロボットの脚をなるべく伸ばしたままの歩行が実現できます。
以前、ロボットの脚を伸ばしたままにできるように、脚長一定で倒れこむ運動を取り入れた歩行パターンを生成したくて、球面振子などを検討してみたのですが、非線形の式になるのでうまく取り扱ことができませんでした。
拘束直線を利用した方法だと、脚長を一定にすることはできませんが、式は線形のままです。

あとは、これを左右の運動も考慮した3次元に拡張すればよいのですが、
支持脚切替による運動制御を、着地の位置ではなく、タイミングの調節によって行っているので、うまく前後方向の運動制御と、左右方向の運動制御が折り合いつくかどうかです。
もしかしたら、まるっきりだめかもしれません・・・。

Top

HOME

tsumehashi

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

01 | 2010/02 | 03
- 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 - - - - - -