ボールの方向に向くように旋回動作をするラムダ■１月３日■
新年あけましておめでとうございます。 今年もよろしくお願いします。
年末は２７日に帰省して、母親の喜寿のお祝いをしてまいりました。 そのため、２７日の練習会と忘年会には出席できませんでした。 残念。 練習会も、普段はこられない人たちが参加していたようで、顔出すだけでも行こうかとも思ったんですが、無理でした。 昨日の夜からUターンだったのだけど、今までで一番車が混んでいて、普段は朝の2時とか3時には家に到着するのに、今回は着いたのが7時。 午前中はごろりと転がって足りない睡眠を取ってました。
来週末はもうわんだほーなんだけど、ラムダの方はさっぱり仕上がっておりません。非常にやばい状況です。
帰省先にまでロボット開発設備一式持って帰って、そこそこは作業したのですが、勝手が違うとなかなかうまくいかないもので効率悪くて進まないし、急にラムダが歩かなくなったりで、あまり進捗しませんでした。
今回、歩行とトラッキングと音声認識を同時に動作させようとしているのですが、どうやらCPUのパワー的にか、プログラムの作り的にかはわかりませんがダメなようです。 さっぱり歩行ができなくなる時があるので色々確認してみたところ音声合成プログラムを停止すれば解消することがわかりました。 とりあえずは音声合成は停止でいくことにはなりそうです。
トラッキングについても、ホワイトバランスがおかしくなる事があり、目標物がさっぱり見えなくなることがしばしば発生します。 はじめどこかの配線の接触不良かと思ったのですがそうでは無さそう。 いまのところ原因不明のままです。 こちらは自律でダッシュ2000の大事なとこなので大問題。 でも、ここにばっかりひっかかってる訳にも行かず、まずは自律で動作できるプログラムを作る方が先決かな。
なんだか新年早々あれもこれもうまく行かず、ぜーんぶほり出してふて寝してしまいたい気分ですけど、ラムダのデビューのためにもがんばらねば。 きっとこれが引退試合になりそうだし。。
■１月４日■
今日で正月休みも終わってしまう。
今日は一日ラムダのプログラム。 しかし、、まだまだ終わりが見えない。 今週休みが取れればもしかしたらなんとかなるかもしれないが難しいなぁ。
■１月５日■
ほんのちょっと、ほんのちょっとずつだけど進捗しています。
ボールの方向に向くように旋回動作をするラムダ
もうすぐパイロンに近づいていくことができるようになりそうです。 そうすれば次はとうとうパイロンの周りを回るという動作に入れます。
プログラムを安定させながら、トラッキング動作の調整をしていたのですが、思い通りに動かない原因らしきものをやっと突き止めてほぼ目標物の方向に向くことができるようになりました。 目標物に近づくことはできていたのですが、目標物の方向に向くということがなぜかうまく行かなかったのです。 原因は旋回動作時に胴体の姿勢値を回転させるタイミングと、目標物の方向を判定するタイミングでのズレでした。 ややこしい姿勢値を採用しているおかげで色んなとこでひっかかります。(^^ゞ
あとは旋回の歩幅までは可変としていないので、目標物に対峙する分解能は粗いです。±0.3ﾗｼﾞｱﾝ（±17degくらい）の範囲に入れば止まるという程度の粗さ。 今回はサッカーするんじゃないのでいいでしょう。
動作が不安定なので、割り込み周期を25msから50msに伸ばしてみたりしたのですが、あまり効果がなく、どうしたものかと悩んでいたのですが、キー入力待ちループにsleep(10ms) を入れるようにしたら随分と安定するようになりました。 トラッキング動作はすごく滑らかになりました。 もっとウェイト大きくしてみようかな。
■１月６日■
アプローチ（目標物に近づく）と、アラウンド（目標物の周りを回る）の動作がだいぶできるようになってきました。 歩行はあまり安定していないのでサポート付きですが。
デバッグしていると時たま「はまる」ことがあります。 カメラの可動範囲を超えてしまってトラッキングできずに固まってしまうとか。 股の間から目標物が見えてどうすりゃいいの？とか。
股の間からボールが見えてる。
こんなときはトラッキングを諦めて旋回動作に移行せねばならない。 そうしてどんどんと複雑になっていくのです。(>_<)
しかし、まだまだまだまだやらなきゃならないことが山積み。 外装は、、、無理だなぁ～(>_<)
■１月８日■
昨日は本部総会でその後パーティー。 ほぼ強制参加なのでパーティーだけ出て帰ろうかと思ったのだが、アルコールが入ってしまうとダメでした。(^_^;) 二日酔いになるくらい飲んでしまった。 なので昨日の進捗はゼロ。 パイロンを回ってからゴールに向かうための手段がまったく出来ていないのだが。
パイロンの周りを回るのは結構難しくて途中で方向修正のために旋回動作をしなければならない。 そうすると歩数を数えてもあてにならないのでどうやって１８０度回ったかを知るか、ゴールを探す動作に移行するタイミングを決める手段が未だ無いのだ。
歩行が安定していないので、なんとしても転倒受身をさせたくなった。 ところが、今積んでいるジャイロは感度が悪くてうまくない。 ようは使い物にならない。 いまさらハードをいじる時間はないので加速度センサーで転倒検出ができないかを試していた。
結果は×。 (>_<) 加速度センサーは転倒時には無重力となるので検出ができない。こけてからモーション発動しても遅いんだよねー。 今日一日そればっかりやってしまて貴重な時間を無駄遣いしてしまった。
明日は、帰るのは早くても11時。 いよいよ徹夜しなきゃならないかもしれなくなってきた。 徹夜したら次の日確実に動けなくなるので絶対やりたくないのだ。
■１月１０日■
とうとうわんだほー前日になってしまいました。
一応プログラム的には完成したけれど、パラメータ調整とかはこれから。
あと、転ぶと自発的に起き上がらな。 歩こうとしたら起き上がるのだが、歩こうとするきっかけは目標物を探しても見つからないか、目標物が見つかったときなので時間がかかる。 転ぶと自発的に起き上がるようにしなくちゃならない。
外装のお面部分は塗装を補修して済ませたがボディはもうぼろぼろなのでプラ板でさっくり作った。 塗装だとすぐはがれるからカッティングシートを貼ったのだが、いい色がなくて形も平坦なのでビブスみたい。(^_^;)
ならばビブスにしちゃおうってことで、背番号ならぬ胸番号入れてみました。 明日はこれでいいや。
■１月１４日■
わんだほー直前のテンパッてた時からわんだほー後の呆けていた期間まで、随分と日誌が更新されていませんでした。(>_<)
去る11日、わんだほーに参加してきました。 ラムダの初のイベント参加です。 結果は35位か36位か、(^_^;) まぁ最下位近辺です。 今回の参加の目的は「自律でダッシュ2000」だったので順位自体は問題じゃあないのですが、そのダッシュ2000、ラムダにとってちょっぴりしょっぱいデビュー戦になってしまいました。
金曜日は友人の送別会で終電間際まで飲んでしまったので作業は出来ず、土曜日の朝からラストスパートです。 ここに来て段々と仕上がりも良くなりバグも順調に取れていっていたのですが、深夜1時ごろ、驚愕の首姿勢の算出関数にバグが発覚。 パン角度が90度を超えると符合が逆転してしまう現象が発覚。 急遽計算式の見直しを余儀なくされました。
でも、パイロンの周りを回る動作が不安定だったのはこの計算式のせいだったのだろうと判ってほっとしたりしてました。
そうこうしつつ、一応形になったのが11日の午前5時。 電源を入れたらプログラムが起動するようにセットして、設定ファイルの確認をして完成としました。 徹夜には耐えられないタイプなので打ち上げのことも考えて3時間くらい寝ることに。
朝起きて最終確認。動作させてみると、あれ？目標物のサーチ動作がおかしい？ フリーズしてしまう？ あせりそうになる気持ちを抑えつつ、冷静に状況分析。。 結果、昨日修正した首姿勢の変換関数が影響してまして、ぎりぎりでバグフィックス。 会場に向かうこととなりました。
会場についたら荷物を拡げて、まずパイロンを借りて色調整をしました。 案の定、パイロンはギリギリ認識するけど、黄色とかわんだほーTシャツにも反応してしまう感じ。 急ぎ修正してなんとかほぼパイロンにだけ反応するようになりました。
ところがところが、どうしたことか、自分の足辺りを見て何かを見つけてしまっている。 もしかしてケーブルの色にマッチしちゃったか？ モニターで確認すると、バッテリーの赤いラインに反応してました。 やばいやばい(^_^;) バッテリーケーブルは黒いビニールテープを貼っておきました。
そして、ついにダッシュ２０００の出走順が回ってきました。 一緒に出走するはずだったマッドハリケーンが棄権なので1台で出走することに。 誤認識の可能性がちょっと減って嬉しかったりして。(^_^;)
1メートル先のパイロンがイマイチ捉えきれていないようだったけど、歩き出せば近づいて見えるようになるだろうと思ってスタートしました。
予想通り、パイロンには順調に近づきます。 そして第二ステージのパイロン周りを旋回。 ところがここからがしょっぱい時間の始まりでした。
パイロンを右に見ながら右にカーブ歩行するプログラムになっていました。 パイロンの位置が右からずれていたらその場旋回して位置取りを修正します。 パイロンはラムダの右のいい位置に見えているはずなのになぜか旋回を、それも左側に旋回を繰り返してしまうのです。 家で調整したときに使った床よりもリングのメラミン床の方が滑るため、こけにくくはなっていたのですけど、歩行も余り上手には行かないのでその場でじたばたしている感じ。
なんとかパイロンの周りを回って欲しくて粘っていたのですけど、特別に制限時間を解いてもらったのにも限度があります。 ラムダは既にパイロンの周りを回るステージに突入していたのでゴールのピンクボールをみつけさえすればボールに向かって進んでくれるはず。 仕方無くボールを持ってラムダの正面側に回って復路ステージに強制突入してもらいました。
結果、イガアさんのサアガが６秒９９で走破する距離を7分以上もうろうろさせてしまいました。 スイマセン。
いしかわさん、わんだほースタッフの皆様そして出場者の皆様にもご迷惑をおかけして、中途半端なパフォーマンスしかできなくて申し訳ございませんでした。<(_ _)>
でも、もし許されるならば次回のわんだほーも出場して今度こそ1分以内くらいにゴールできるよう、リベンジしたいと思います。
あと、もしラムダがパイロン周りをのたのたと歩いている画像持ってる人がおられましたら画像いただけないでしょうか～。
■１月１５日■
って書いて日誌をアップした15分後にKさんのブログにラムダの画像がアップされてました。(^・^) タイムリ～♪
そして、イガアさんからの情報でIKETOMさんのYOUTUBEチャンネルにラムダの「自律でダッシュ2000」の全行程がアップされていることを知る。 IKETOMさんありがとうございます。 見てみると、もう、なんだか恥ずかしいくらいヨタッてますね、ラムダ。
ラジコンで歩かせる時は、ちょっとこけそうか？って場合に歩行を止めて加減したりできるのだけど、ラムダは自律でそれをやってます。 加速度センサー値の暴れ方を見ていて、暴れ方がひどいとこけそうだってことで歩行を停止します。 この閾値をちょっと低めに設定してまして、恐らく1歩歩く毎くらいにそれが働いていたようです。 ちなみにラジコンで動かすときはその機能はキャンセルしていました。 動作については全てログを取っていたんですけど、そのことすっかり忘れてまして、ログをダウンロードする前に電源を落としてしまった(>_<) なので動作の解析ができませんでした。orz...
歩行の方向の修正は1歩ごとに行っていまして、目標物の方向を反映してカーブ歩行の半径を変更していました。 これも1歩でフィードバックするのではなく、数歩レベルでフィードバックすべきだったかもしれません。
次に「自律で・・・」でイベントに出れそうなのは4月の終わりのわんだほー・ぷち。 多足でもいいらしいからぴしいさんが言うようにシグマで自律で、安定してダッシュ2000させてみるのもいいかも。 ラムダはデビューしたけど、関節がガタガタなので足構造の見直しをするつもり。 次回のわんだほーまでにはラムダマークⅡで出場したいなぁ。
■１月１８日■
金曜日は統括部の新年会でしこたま飲んで帰った。 ので、、土曜日は二日酔いで一日どよ～んとしていた。 どよんとしながらも年末からやらなければならなかった部屋の片付け、わんだほーを優先にして後回しにしていたのだが、それをやったり。 この部屋こんなに広かったんだなー(^_^;)
わんだほーにて、ラムダで初めてイベントに参加したわけだが、競技自体とは違ってイベント参加という点で準備しておかなければいけなかった点がいくつかあったので改良していきたいと思う。
ひとつはコンソールなしでのプログラム操作の問題。 もう一つは姿勢設定のコントローラ操作化。
ラムダのコントローラRPU-100はNetBSDというUNIXOSが乗っている。 OSを使うってのは複雑なプログラムを処理するにはいいのではあるが起動が大変で、イベントでは電源を入れたり切ったり運営との兼ね合いもあって突発的な状況が多くあるのだが、UNIXOSだとこれが非常に不便。 OSが立ち上がるのに数分かかるし、ちゃんと設定しておかなければロボットのコントロールプログラムの起動を行うためにいちいちコンソールでコマンドを入れなければならない。
今回は、電源を入れて、OSが立ち上がると初期設定スクリプト上でラムダのコントロールプログラムを起動するようにセットしておいた。 普段はプログラムをRAMDISK上にロードして動作させるのだが、CFカード上のフラッシュメモリーから起動するようにしました。 もちろん各種設定ファイル（基本コンフィグ、カラーテーブルファイル、モーションファイル、ビヘイビアファイルなどなど）もフラッシュメモリーにセーブしておいた。
全てセットして行ったのだが、会場に行ってから調整を行ったりしたのでその都度ファイルの更新をした。 パイロンのカラーテーブル調整を行ったので、カラーテーブルファイルを更新し、ボトルトラクション用に歩行姿勢設定ファイルを更新し、最後にはプログラム本体にも修正を加えて更新した。
用意していった状況で困ったのが、調整段階で、電源を入れたら動作プログラムが起動してしまうこと。 もちろん、まず起動しないようにしてから調整すればいいのだが、調整してファイルを更新するのが前提であればそれでもいいが、問題なければヘタにいじりたくない。 こりゃ調整しなきゃダメだなーということになって調整するわけだから、まず起動しないようにするってのは良い策とは言えない。
問題は起動したプログラムを終了させる手段を用意していなかったことで、いちいちコンソールで入ってプロセスを殺して、、設定ファイルをRAMDISK上に置いてそれを使うようにして、、という煩雑な手順をふまなければならなかった。
ということで、プログラムの停止をコンソールを使わずに行えるようにすべきかと思う。 できれば起動も行えるようにしたいのだが、簡単にできるかどうかわからない。 RPU-100には押しボタンスイッチが2つあり、これらをその操作に割り当てることができるかと思うのだが、デバイスを複数のプログラムで操作するのは難しくて、ラムダ側でデバイスを使うため、プログラム起動・停止のモニタープログラムに操作を渡せないかも知れない。
もう一つの姿勢設定のコントローラ操作化は、ボトルトラクション用の歩行姿勢チャンネルは組み込んでおいたのだが、事前に調整できなかったので当日会場で初めてかごを押すことになった。 結局、ぶっつけ本番でかごを押すことになったのだが、 用意しておいた歩行姿勢はまったくかごの大きさにあっておらず、ぎりぎりまで調整する羽目になった。 ラムダの歩容生成は計算で行っているので、パラメータ設定さえ行えばそれなりの歩容を作ることは出来る。 ただ、その調整にはコンソールやらエディタやらが必要で、当日は自分の席で値を修正してはかごに合わせてみて、だめならまた席に戻っての繰り返しだった。
実はこの状況は予想していて、コントローラで歩行姿勢の調整を行うモードを作りかけていたのだが、時間が足りず、途中で放置していたのだ。 こういうのもちゃんと仕上げておかないとねぇ。
今日は一日、ラムダマークⅡの構造検討。 骨子はできたのだが、すんごーいかっこ悪い。 これほんとに作るのかなぁ(ーー;)
■１月１９日■
RPU-100のスイッチデバイスが複数のプロセスで利用できるかどうかのテスト。
カメラデバイスは排他でしか使えなかったのだが、その他のデバイスはどうやら複数のデバイスで利用可能のようだ。（もちろんRS485も排他利用必須） これでモニタープログラムを常駐させて、ボタンを押したらプログラムの起動をするって操作が出来そうだ。プロセス増やしていいのかなって気もするが、CPU占有時間をうんと短くすれば影響は無いだろう。
ラムダマークⅡの構造、 ちょっと見直し。 かっこ悪さはちょっとだけ減ったが、まぁこれは致し方ないかなぁ。
何をやろうとしているかというと、足の末端質量の軽減と剛性の向上。 ラムダがいまいちしゃきっとしないのはパワーウェイトレシオの問題が大きいのではないかと考えている。 今の設計は、できるだけ身長を低く作りたかったため、質量を分散させようとしていたのだが、倒立振子の運動を採用するなら末端質量は論理的にはゼロなわけで、足先が重いのは非常に良くない。ZMP規範の制御をしたとしても末端質量が小さい方がいいだろうということ。 まぁ基本なのですが、ラムダを設計したときはなんとかなると思っていたわけです。
本格的な次機種は違うサーボの採用を考えているのだけど、今の材料で改善の目安をつけておきたい、ということです。
改善できたらいいなぁ。
■１月２５日■
先週は会社でインフルエンザが大流行。 月曜日に会社に行くと周りだけで４人が病欠。うち２名はインフルエンザ。 そして具合の悪いのが１名で、すぐに早退して病院に行くとやはりインフルエンザ。 更に風邪申告の２名に病院に行くように言うと、１名は陰性だったんだけど、もう１名はインフルエンザ。 (>_<)
もうこうなると、周りを見渡すとインフルエンザウィルスが見えそうな勢いで、自分も具合悪くなってきた。
更に木曜日、早退した１名の隣に座っていた１名がインフルエンザ発病。 どんだけ感染力あるんだよ！ そいつとは火曜日一緒に飲みに行っちゃったんだけど、発病する前ならウィルスばら撒いていないはず。
結局、月曜日に休んだインフル４名は全員喫煙者。（陰性だった1名も喫煙者） オフィスビルは基本禁煙で、喫煙するのは小さな喫煙ルームのわけで、ただでさえ免疫力が落ちている喫煙者が小部屋で一生懸命息吸って吐いているわけだから感染するのもうなずける。
ちなみに喫煙者なのに感染しなかった人が一人だけ。 そいつは長男が今年受験だってことで家族全員予防接種を受けていたのでした。 今年はワクチン外れって聞いてたけど効いたのかな。
２４日はROBO-ONEテクニカルカンファレンスだったんですね。全然ノーマークで知らなかった。 お題がお手伝いプロジェクトだったからまぁよかったかなぁと、ただ網野さんのプレゼン聞きたかったな。
今後のロボット開発の活動計画を考えるのに、ラムダの歩行改善が大きなテーマ。 下半身の構造変更が大きな課題なのですが、制御方式もワンランクアップさせたい。 近頃、普通の会社員の日記の西さんがZMP規範の歩行生成で大きな成果を上げておられますが、ラムダの歩行も倒立振子軌道の再生からZMP規範の歩行に移行していこうかと思います。
そもそもZMP規範も倒立振子も含めた動力学計算による歩行生成じゃない方法で歩かせたいなと思って遠回りしたのですが、センサーが少ない現在のパーソナルロボットだとやはり動力学を使って制御するのが一番のようです。 倒立振子は直感的に理解しやすかったのと、解析解があったので計算量が少なくていいからまぁこれでいいかと思っていたのですが、倒立振子軌道は初速度がでかいので、やはりサーボのパワーとか再生精度で問題があるというのが結論です。 動力学使ってるくせに計算量減らそうってのが虫のいい話だったんですね。
その他にも足構造の見直しやら、姿勢再生の補正手段やら折り込みたい事があるのですが、ラムダマークⅡの構造検討をしながら、ZMP規範の歩行生成やら姿勢再生の補正やらをプログラムしてマークⅠ機体で実験していくって感じの進め方になるかなぁと思ってます。
西さん（と馴れ馴れしくお呼びしていますが面識はありません。スイマセン(^^ゞ）がブログで紹介しておられた、三重対角連立方程式の解法を解説しているページを参考にして、オフライン歩行パターンの生成をやってみました。 ちなみにこのページの数式は結構な間違いがありますので注意が必要です。数学理論はさっぱり判りませんが、やっていることは複雑ではないので順を追って自分で計算した方がいいみたいですね。
紺色が目標ZMPで赤色が重心軌道です。面白いですねー。 本を斜め読みしているときはよく理解できませんでしたが、数式を順を追って計算してみるとなるほどなるほどという感じです。
目標ZMPの軌道（といっていいのかな？）を離散化して数列にします。この数列を行列 P とし、これが成り立つ離散化された重心座標列を行列 X とします。この関係を成り立たせる行列を行列 A として
P=A・X
これを解いてXを求めればいいわけなのですが、そのときの行列 A が三重対角行列になるようにZMP方程式を離散化近似式にしているわけです。
三重対角連立方程式を解く時にLU変換といって、更に簡単な2つの行列に分解します。
A=L・U
これを前式に代入して
P=L・U・X
これを、解くのにZという中間の行列を考えて、
Z=U・X
そして、
P=L・Z
とします。Lは簡単な行列なので、Lの逆行列を計算しなくともZを求めることが出来ます。 そして、上のZ=U・Xを同様に計算すればXを求めることができます。
Zを求めるときは i=1からｎに向かって求めて行き、Xを求めるときは逆にi=nから1に向かって定まって行く様子がなにやら目標ZMPに向かっていく様を表しているようで面白いですね。
「ヒューマノイドロボット」ではこの三重対角連立方程式を解く部分をさらりと流していますが、もうちょっと解説してくれていればもっと敷居が下がっただろうにと思いました。
これで初速度ゼロから動作を始めて速度ゼロで動作停止させることができます。
さて、ここまでで単質点のZMP規範の重心軌道は求められたわけですが、歩行モーションとするにはロボット全体の運動による影響を補正しなくてはなりません。もうちょっと力学の勉強が続きます。
上記のグラフのエクセルファイルをリンクしておきますので参考にしてください。
■１月２６日■
単質点での重心点の軌道が算出できたので、次は多質点モデルへの補正です。 そのためには任意の姿勢でのZMPの算出を行う必要があります。 これは「ヒューマノイドロボット」の３．４ロボットの運動からZMPを計算する の部分の計算をします。
ラムダは既に多質点モデルを持っており、関節角度からその姿勢での静止重心の計算をできるようにしています。 ZMPの算出を行うには、ロボットを動かすための関節角度列を再生する際の、各質点の三次元座標と速度ベクトルと加速度ベクトルが必要です。角運動量の影響も計算するならば、質点の姿勢と角速度ベクトルも必要となります。あと、質点の質量と慣性テンソルが要ります。
インベンターの物理量計算の機能で、慣性テンソルの算出はできるのですが、姿勢から角速度を求める計算がわかりません。(>_<) バイブルによると、角運動量の計算はしなくとも実用には耐えるとも書いてある。
角運動量を考慮している 「３．４．１ ZMPの導出」 の最後の式と、角運動量を考慮していない「３．４．２ 近似によるZMP計算」に出てくる式では表記レベルに差がある。 これはどうやら読者には３．４．２の方を薦めているように見えるなぁ。
ではお言葉に甘えて近似式の方で進めることにします。
式3.71、式3.72 に式3.75を代入したら式3.76、式3.77になるって書いてあるのだが、あれ？展開しても合わない。
これは次のページの式3.81を参照すれば展開して計算があうのがわかります。
L=ｃ×P
となっているので
L={Lx,Ly,Lz} c={cx,cy,cz] P=｛Px,Py,Pz｝
として、うっかりと
Lx=cx×Px
としてしまっていました。×は積じゃなくて外積（ベクトル積）をあらわしているんですね。
角運動量は位置ベクトルと速度ベクトルの外積なのでっていうのが正しい表現かもしれないですけど、力学に慣れ親しんでいないので直感的に判りにくいです。 角運動量ベクトルの要素はその座標軸周りの回転運動の大きさを表しているので、LxはX軸からcy離れたところでPzの力で回転する力と、ｃｚはなれたところで-Pyの力で回転する力の合計なので
Lx=cy×Pz-cz×Py
となります。
本の前の方には外積の話も出てくるんですけど、（そこでは外積は ＾ で表しますって書いてる） もうちょっとひつこく説明して欲しかったかなー。 理解しているつもりだったんですけど、身についていないもんだからすっかり間違えてしまいました。
ちなみに質点の速度は前フレームとの座標の差を取ってフレーム間時間（⊿ｔ）で割る。 質点の加速度は前フレームとの速度の差です。 角速度は、、回転行列から算出するみたいですけどまだわからんです。
プログラム書くつもりが日誌書いてたら時間になってしまった。(>_<) 本末転倒だ。 ええと、表現や理解に誤りがあったらぜひ教えてください。
■１月２７日■
今日は残業、明日は早朝から大阪へ出張 (ーー;)
「多質点モデルのZMP導出」のプログラムが着手出来ない～。
ラムダの持っている多質点モデルでの重心計算は、静止重心の計算をするために四肢の先端から根元に向かって質量を合成していく作りになっていたので、質点それぞれの姿勢や座標を計算するとなると、スキャン方法を変更しなきゃならないかも。 あれ？慣性モーメントの計算はどうしていたっけ？
ええ～と、慣性モーメントの計算ルーチンがそのまま使えればちょいと手を加えるだけで出来そうな感じだな。 早く計算してみて単質点での目標ZMPと多質点でのZMPの差分を見てみたい。
明日の帰りの新幹線で、あまりビールを飲まないで帰ってこれたら明日できるかも。(^_^;) 無理か。。。 (^・^)
■１月２９日■
昨日、大阪から新幹線にのったのが、19時19分 新横浜に着いたのが21時30分。 ビールは飲んだけどさほどでもないので帰ったらプログラム～♪ と思って横浜線ホームに行くと、、 なんだかおかしな様子。 電車故障で全線ストップ。
東横線に乗り換えるからお隣の菊名まで行ければよいのだが、動く気配がない。。 横浜市地下鉄で迂回する気にもなれないので、30分ほど待ったのだけど、諦めて飲みに行くことにしました。(^^ゞ
1時間もすりゃぁ動くだろうと思ったら、、、 11時20分くらいにホームに行くと、まだ動かない様子。 あらららら、諦めて横浜市地下鉄で迂回しました。こんなことならさっさと迂回して帰ればよかった。 武蔵溝口まで出て南武線に乗り換えるのだけど、 あざみ野から田園都市線に乗ったら、今度は「渋谷で人身事故発生」 な、なにぃ～、、南武線終電やばいやーん。
ちょっとあせったけど、程なく動き出しました。 二子玉川園までは行ってくれるらしい。 よかった～。
でも、帰ったらもう12時も回ってしまっていたのでさすがにプログラムは出来ませんでした。
そして、今日。 今日も残業で遅かったのだけど、今日こそは多質点も出るのZMP導出をプログラム。 その前にネット巡回を。。。 おぉぉ～、「普通の会社員の日記」が更新されてるぞ。どれどれ、、、 と見に行ったら、、、、西さんここ見てくれてるんですね。(*^_^*) ありがとうございます。
こないだの関東ロボット練習会は年末で、予定がどうしても合わなくて行けなかったんですよね～。残念でした。 大抵は参加しているので機会があればお会いできると思います。
で、ZMP導出。
重心管理クラスのGravityにGetZMP（）って関数を作ってちょちょいと作ってみました。 ある姿勢での任意軸の慣性モーメントを計算するGetMOI（）を改造しようと思ったのだけど、これまた妙なことをしていたので、別物で素直な感じで作りました。
さぁ動かしてデバッグだー、と思ったのだが、デバッグするために食わせるデータを作るプログラムを作ってなかった。 てか、重心軌道の生成もエクセル上で作っただけで、ラムダ上では作ってなかったんだった (>_<)
順番が逆だけど、多質点モデルでのZMP導出関数のコーディングは出来た（未デバッグ）ので、次は単質点の目標ZMPへの重心軌道生成部と、ロボットの姿勢生成のコーディングです。(^^ゞ
まーこれはとっかかればすぐですね。 とっかかれるかどうかが問題。
明日も帰るの遅そうなので土日に突入しちゃうかなぁ。
■１月３１日■
昨日は思ったより早く帰ってきたので目標ZMPへの重心軌道生成の関数を作ってました。 エクセルではX方向だけ作ってましたが歩かせるにはY方向も必要ということで両方一緒に計算する関数を作成。
多質点モデルは自作クラスをたくさん使っているので、なにやってるか判りにくいからソース広げても意味ないかと思いましたが、こっちの方はそういうこともないので載せてみます。エクセルファイルでは数式の展開手順をそのまま計算したので無駄な部分も多かったのですが、プログラムにすると関係ないところは極力省いて書いてます。C++でSTLも使っていますけど簡単な機能しか使っていない（自分が使えないので(^^ゞ ）のでなんとなくは判るのではないかなぁと。
この計算って、ワールド座標上での計算になるので、歩き続けるにはちょっと工夫が必要。止まらずにカーブ歩行に移行したりするケースも考えなければならない。
さて、次はこの重心軌道を元にロボット姿勢を生成して、多質点モデルでZMPを導出してみます。
■２月１日■
やっとできた～ (>_<)
多質点モデルでのZMP導出です。 グラフ中のｍZMPってのが多質点モデルでのZMPの導出結果です。教科書（ヒューマノイドロボットのこと）ではロボットの前進方向がX軸で、左右がY軸なんですが、ラム研では左右がX軸で前後Y軸です。
実はまだ歩行モーションになってなくて、腰を左右に振っているだけで、足踏みさえしていません。 なので、Y軸（前後方向）は重心点のズレによって生じるモーメントでZMPにズレが出ているようです。
コーディングはちょちょいと作ったけど、デバッグはちょちょちょちょーーーーーい、くらいかかりました。(ーー;)
はじめ、ロボットの原点を固定したままで足が動くような計算をしてしまい、しばらくはまってしまいました。 あ、っと気付いて修正するとき、何を勘違いしたのか重心点の微分値を各質点に加えてしまって更にハマリました。 ロボットの姿勢関係はローカル座標で管理しているのですが、この一連の計算はワールド座標なので、すべてワールド座標に変換しなければならないのです。 やれやれだぜ。。
ちなみにハマっている最中に作ってみた単質点でのZMP導出がこれ↓
⊿ｔも小さくしてみました。 意外とズレが大きいです。重心軌道の計算もZMPの計算も近似があるから仕方ないのだろうな。
↑※重心軌道算出時と、ZMP算出時で重心高さに違う値を使ってました。 ずれが大きいのはそのためのようです。（2009.2.1）
さて、次は補正です。 西さんは1回で十分と書いておられましたが、ラムダは全身なので、誤差が大きいような気もします。どうなるか楽しみ。
多質点モデルでのZMP導出の関数も載せてみましょう。 独自クラスを使っているのでその辺りは感覚で読んでください。(^^ゞ 質問は有りですのでBBSまで。
朝になりました。 このまま補正に突入したいところなんですが、進行方向の計算が全然できていないので歩行生成についての検討をします。
目標ZMPの軌道と、足の位置及びロボット原点の関係を図とグラフにしてみました。 左の座標図の緑の丸がZMPの位置です。 目標ZMPは床面を点々と移動していきます。赤い矢印線が移動の順を示してはいますが、この赤い線上に目標ZMPが存在するという意味ではありません。
紫っぽい色の線がロボット原点の軌跡です。ラムダの姿勢では、ロボット原点は常に両足の中点にあります。そこからずれる分はオフセットパラメータなどで表現しています。直進に歩行する場合、ロボット原点は常にY軸上にあることになります。
目標ZMPから求めた重心軌道ではY軸の動き（前後の動き）は、ほぼ直線だけど、脈動している感じでした。 あの軌道をそのままロボット原点の動きにすることもできるのですが、遊脚軌道を考えた場合、時間に対してリアルな動きになるようにした方がのちのち考え易いと思い、ロボット原点の動きはリニアにして、重心位置はオフセットで与えることにします。
遊脚を上げられるのはZMPが支持脚側にある間です。赤いライン部分が遊脚復帰期間となります。
そして、遊脚復帰動作ですが、ラムダの姿勢パラメータで表すと、前後の足間隔と足上げ高さになります。 これは理想的な動きで、両足支持期がないのですが、実際には足上げ高さが０の時間を数フレーム設ける必要があると思います。
やっとこさで歩行パターンができました。 なんとか美しいインプリを考えていたらあっという間に夕方になってしまった。 結局はひとまずべた書きに近いテスト専用の歩行パターンです。足上げカーブは単純にサインカーブを使いました。
ごちゃごちゃしたグラフですが、午前中に書いた遊脚運動が再現できています。 そして、この動きをさせたロボットの関節運動から多質点モデルでのZMPを計算すると、
こんな感じです。 昨日と違うけど、昨日のはちょっとパラメータを間違えていたから足上げの影響とは言い切れない。
さて、やっと補正に取り掛かれます。
補正です。 教科書どおりにやってもうまく行かなかったのでアレンジしてうまく行くようにしてしまいました。 なにかバグがありそうなので詳細はもうちょっと検討してからにします。
という問題を抱えながらも、２回補正をかけたらほぼ目標ZMPに近い形になりました。Y軸で値が暴れているところがありますが、重心軌道は暴れていないので問題ないような気がしているんだけど、甘いか？
今までのラムダの歩行では、静止重心を計算して、その結果の重心に対して倒立線形振子の軌道を与えていました。 今回の目標ZMPでは多質点モデルでのZMP導出において同様のことを行うため、ロボットの構造上の論理原点を重心点として計算しました。 Z軸方向だけは基本姿勢での重心高さを考慮しています。
静止重心で、左右に-3ミリ、前後に-11ミリくらいのズレがあるのですが、それに近いオフセットがでてきているのでまぁ計算はあっているのかなぁと思います。
ただ、2回補正した結果、左右振幅がZMPのふり幅を超えています。いいのかなぁ。 経験では横にすっ飛びそうな気がしますが。
これで歩行のためのデータは出来たのですが、まだ歩かせることはできません。 それは、関節角度の再生精度を上げなければならないという問題があるからです。 せっかくこれだけキチンと計算しても再生できなければうまく歩けないのは道理。 これについてはアイディアレベルのものがあるのでそのインプリと実験をしなければなりません。 うまく行かなきゃラムダMARKⅡにかけるしかない。(>_<)
■２月３日■
今週は早く帰ってこれる日がなさげなので開発作業はできなさげ。
ZMP規範の歩行生成は面白いですね。 倒立振子とは違って、あらゆるモーションのZMP補正値を求めることができるわけです。 サッカーのキックモーションで、ZMPを設定してモーションを流して補正をかけたら足の振りに合わせて重心を動かすようなモーションに修正されるはずです。
今までのラムダでは静止重心を同様の手段に使っていたのだけど、キックモーションのような大きな加速度が発生する動作には効果が無かったのですが、ZMPだとそれにも対応できるはず。（サーボの応答特性があるからなんでもOKってわけじゃないだろうけど）
静止重心だけでも結構便利だったので、期待です。
多質点モデルでのZMP導出を使った重心軌道の補正で、2回まわしてなんとかなったって話ですが、静止重心の考慮を入れておいたら1回で済むって話はないでしょうか。 前後は10ミリ以上のズレがあるから差分があるでしょうけど、左右は3ミリ程度しかないからそんなに大きな効果は期待できないですねー。 でも、止まった時の姿勢との整合もあるので静止重心の考慮は入れておくようにしようかな。
再生精度の向上は、基本的には「補正」をかけるって話なんですが、補正テーブルってのを作ってみようと思ってます。 でも、補正値は関節に対して一意なのではなく、関節と、関節角度と角度の移動速度と、そして機械的ガタ付きについては角度の加速度が影響します。 関節角度が補正値に影響するってのは角度の加速度が影響するってのに近くて、つまり重力加速度がどれくらい影響するかって話です。
更には、例えば膝関節だと、指示脚の時と遊脚の時ではフリー端が逆になるから加速度のかかり方も逆になるわけで、補正値も変わります。ま、遊脚の膝角度はそれほど重要じゃないからきにしなくてもいいかもしれませんが。
■２月７日■
案の定、先週のウィークデーは毎日帰るのが遅く（とは言ってもうち２日は飲みに行ったせいでもあるのだが）ロボット開発はまったくできませんでした。
さて、再生精度の向上策の検討と実験です。 まずは静止状態での補正をみます。
左から 補正なし 補正有り しゃがんで補正有り
主に膝関節の補正が効いているのだと思われるが、補正をかけることで胴体が垂直になっているのがかろうじてわかる。 実は実験を始めた時、補正をかけてもさっぱり胴体が垂直にならないのでチェックしてみると、股関節のブラケットが歪んでしまっており、それが原因で胴体が傾いていたらしい。 ブラケットのひずみを直すべきなんだろうけど、とりあえずは定義記述でブラケットのひずみを正規値とおいてしまいました。
関節角度と補正の様子を見ると、
立ち姿勢
joint[0] t:0.153589 a:0.153589 diff:0.000000 adj:-8
joint[1] t:-0.382227 a:-0.385718 diff:0.003491 adj:-3
joint[2] t:0.000000 a:-0.005236 diff:0.005236 adj:0
joint[3] t:0.863938 a:0.867429 diff:-0.003491 adj:-12
joint[4] t:-0.479966 a:-0.479966 diff:0.000000 adj:-3
joint[5] t:-0.153589 a:-0.153589 diff:0.000000 adj:2
joint[6] t:0.157080 a:0.157080 diff:0.000000 adj:0
joint[7] t:-0.380482 a:-0.380482 diff:0.000000 adj:-10
joint[8] t:0.000000 a:-0.005236 diff:0.005236 adj:4
joint[9] t:0.860447 a:0.863938 diff:-0.003491 adj:-13
joint[10] t:-0.478220 a:-0.476475 diff:-0.001745 adj:-9
joint[11] t:-0.157080 a:-0.157080 diff:0.000000 adj:-8
しゃがみ姿勢
joint[0] t:0.240855 a:0.240855 diff:0.000000 adj:7
joint[1] t:-0.921534 a:-0.928515 diff:0.006981 adj:-6
joint[2] t:0.000000 a:0.000000 diff:0.000000 adj:9
joint[3] t:1.895428 a:1.890192 diff:0.005236 adj:-16
joint[4] t:-0.973894 a:-0.979130 diff:0.005236 adj:7
joint[5] t:-0.240855 a:-0.240855 diff:0.000000 adj:-10
joint[6] t:0.247837 a:0.246091 diff:0.001745 adj:-5
joint[7] t:-0.919789 a:-0.916298 diff:-0.003491 adj:-10
joint[8] t:0.000000 a:0.000000 diff:0.000000 adj:10
joint[9] t:1.893682 a:1.886701 diff:0.006981 adj:-23
joint[10] t:-0.972148 a:-0.970403 diff:-0.001745 adj:-9
joint[11] t:-0.247837 a:-0.246091 diff:-0.001745 adj:-7
関節番号は０から５が右足で６から１１が左足。 股関節から足首関節に向かって０→５となっています。
膝関節はjoint[3]とjoint[9]。ここに着目すると、しゃがみ姿勢の方が補正角度が大きいのが判ります。関節のガタ付きは角度によって増減しませんが、関節に対する負荷はしゃがみ姿勢の方が大きいので、負荷の大きさの違いが補正角度に現れたと考えられます。
歩行だけを考えると、これほど大きな姿勢の変更はないので基本となる補正値は歩行の基本姿勢の静止時の補正値でもいいのかなと思います。関節のガタ付きが逆転するのは多分支持脚が遊脚に切り替わった時だけだし、遊脚の再生精度は支持脚ほどはシビアじゃないからとりあえずはいいだろう。 動作に関して動作速度と負荷が変わるため、遅れが生じるのでこれを補償しなければなりません。 各関節の目標速度と目標加速度と位置は判るので、関節にかかる負荷はわかるはず。
問題は関節の予測負荷に対して関節に相当のトルクを与えるということができるのかというところです。 PID制御されているサーボモーターは位置を制御するもので、トルク制御まではできないし、なによりステップ応答以上の応答速度はどうやっても得られない。
まずはとにかくどのくらい遅れるのかひずむのかを確認してみよう。
■２月８日■
遅れ・ひずみ測定に入れないでいます。
というのもZMPの計算結果に不安があるからで、昨日も終日その検証をやってました。 多質点からのZMP導出の計算はこれで合ってるの？ということなんですが、どうやら合ってるみたいです。まだ納得しかねているところも有るので検証は続きそうです。
結果を安定させたいと思って、時間分割を細かくするともっと値が暴れだす。 これは計算を倍精度にすることである程度は抑えられました。 時間も計算精度も含めて精度を上げると補正は1回でもいいかなーというくらいになってきました。 計算時間的にはどっちがいいのかわかりません。
そうしてもまだZMPが急変する点があってそれを調査しています。
ZMP（Y)がZMP(X)の急変点で影響を受けている。これはなんだ？ということで調べてみます。
ZMP算出途中のいくつかの値を見てみると、急変点があります。これがZMPに見えてきているのでこの値の原因を追っていくと、
左の膝辺りの質点に急変部分を発見。
座標の動きはこんな感じ。 1歩目は左足が遊脚になるので遊脚復帰動作らしい。 意外と影響が大きい。 やはり足の末端重量を軽くしなくちゃならないのだろうか。 両足支持期間を設けたり、今はリニアに復帰している遊脚運動を加速するようにするなどの対策は考えられるけど、どうなんだろうなぁ。
時々計算結果が不定値（NaN）になることがあって、それが再現性がなくて困っていました。 変数が初期化されていないのか、ポインターの問題か、 でも、C++でSTLを使っているので変数の初期化もポインターも問題ないはず。 膨大な計算のログを取って調べたところ、タイマー割り込みを止めると発生しないような気がする。 少なくとも明らかにタイマー割り込みを使っている時は発生頻度が多い。 でも、割り込みで計算結果を壊すようなところは無いはずなんだけどな。
タイマーを止めてしばらく様子を見て、発生しないようなら更なる原因究明に入ろう。
ZMP計算のループにスリープを差し込んでみたら、計算ミスは激減した。 処方後に1度だけエラーデータが出た。 根本的解決策じゃないから仕方ないか。
遅れやひずみを見るためにとりあえずは計算したデータで歩かせるプログラムを作製。 やっぱりさっぱり歩けません。横に吹っ飛んで行きます。 ま、予想通りなのでめげずに遅れ・ひずみの様子を見るために関節角度データを採取してみました。 データ中、カクカクしてるところはデータ採取ミスです。 「ｔ」が目標で「m」が取得角度。 取得角度は指示してから50ms後のデータです。一致しているようだけど、実際は50msずれています。
↓まず股関節ロール軸。 1が右足で7が左足、角度がマイナスになっている時が支持脚になっているときです。 支持脚の時にオーバーシュートしてしまっているのがわかります。 吹っ飛ぶはずだ。
↓次に股関節ピッチ軸。 ほぼ遅れはない。 意外だな。-200から-250辺りが支持しているときなので、角度のズレが見えます。
↓これは大腿にあるヨー軸。 今回の歩行パターンでは常に0なのに、支持脚となってる時にガタ付きによるズレがでているようです。 すごいずれているみたいに見えるけど2度未満ですね。
↓これは膝関節。 遅れはないけど、支持時の角度ずれが顕著。 姿勢のズレにつながるので後ろにこける原因になってるようです。
↓足首ピッチ軸 支持時に角度ずれが顕著。 遅れはそれほどないように見える。 支持時にズレと遅れが混ざっているかもしれないなぁ。
↓足首ロール軸。 股関節ロールと同じ感じでオーバーシュートが発生してます。 いままでは足首ロール軸はコンプライアンススロープ設定にしていましたが、再生精度の検討なのでコンプライアンスは入れていません。 荷重でこれだけオーバーシュートするんだなぁ。 足裏は床面からはがれてしまっているので角度再生はできているかも知れないと思っていたがそんなことはなかった。
遊脚復帰に遅れがほとんどないのは意外だけどもうけもの。 ピッチ軸がオーバーシュートしているのは予想通り。補正でどれくらい再生精度を上げられるか、だなぁ。
その他、遊脚復帰動作もなにか考えなきゃならなさそうだし、まだまだ道は遠いなぁ。