わがロボットの譜

千葉工業大学、総合工学研究会所属の者です。とりあえず、二足歩行ロボット作成日誌です。

スポンサーサイト

上記の広告は1ヶ月以上更新のないブログに表示されています。
新しい記事を書く事で広告が消せます。

PageTop

学校がある時ってこんなに作業が進まなかったっけ…?

まぁ、学年末だからですかね。


この前、重心の左右の軌道を描いたので、そこからテキトーに逆運動学するプログラムを書きました。
脚を挙げてても重心高さは変わらないって体で、足は曲げ角と開き角だけ。。

で、なんだか授業でニュートン・ラプソン法で連立方程式を解かなきゃいけないみたいなので、それで作ってみました。

「ニュートン ラプソン C言語」 とかで検索すれば出てくるかと思ってたけれど、以外に出てきませんでした。
連立じゃなければいっぱいあるのだけれど…


とりあえず、プログラム
逆運動学…?

「.csv」から重心の目標位置を読み込んで別の「.csv」に出力します。
これをマシンに突っ込めば、足踏みをせずに重心を横に移動していく気持ち悪い動きができる…はず。


で、ニュートン法について。ニュートン法は変数に毎回補正値をかけて、収束を待ちます。
f1(x,y,z), f2(x,y,z), f3(x,y,z)がある時、
数式1
こんな風にf1,f2,f3を変えていって、収束を待つわけです。
偏微分の塊はヤコビ行列なので、まぁ今後はJとかで書きます。

で、δx,y,zっていうのが各変数に対する補正量になるわけです。補正量は求めなければなりません。
f1,f2,f3 = 0を目標にすると、左辺は0にできるらしいです。
プログラムでは、目標値を計算したかったので、右辺から目標値を引いています。

数式2
そんなこんなでこうなるらしいです。

で、結局ステップとしては、
まずヤコビの逆行列を求める(私はMATLABでやりました)

x,y,zの初期値からf1,f2,f3を計算

δx,y,zを算出

補正量をx,y,zに足す

f1,f2,f3を計算

収束するまでループって感じです。

自分が思うにx,y,zの並びも割りと重要だと思います。ぼーっとしてたら間違えそうです。
そこらへんの資料だときちんと行列表記されていないので、わかりづらかったので書いてみました。


歩行モーションのデータは、とりあえず5msでデータとって見ましたが、今のところはもっと荒くてもいいぐらいしか動いていません。
まぁ、まだ歩行の一要素しかないので当たり前ですが…
実装はどうしようか、そのままテーブル見たいに突っ込んでしまおうか…?

…たぶんそうすると思います。変数領域が…






スポンサーサイト

PageTop

コメント


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

上記広告は1ヶ月以上更新のないブログに表示されています。新しい記事を書くことで広告を消せます。