開発日誌

今日がページの変え時か？　という話なんですが、「なんかページが長いなぁ～」と思ってしまった。思い立ったが吉日、ということで改ページしました。

これから各サーボに補正を加えて再生精度を上げなくちゃならないのだけれど、どういう仕組みにするか考え中。

なんせ、計算歩行が売りなので歩行モーションに対して補正値を持つって仕組みはどうなのよと、まぁやるとしたら自己学習で補正テーブルを自動構築とかなんだけど、関節角度の取得の失敗率の高さからその手段は取れなさそう。　やはり、各サーボにかかる負荷を計算で出して補正値を算出というスタイルが理想的かなと考えています。

その仕組みはさておき、まずはZMP規範にて生成した歩行モーションが使えるのかどうか。マニュアルでもいいから補正を加えて歩かせたいなと思ってます。　サーボの角度列を再生するプログラムが無いのでどうしたものかなぁ。　作るの面倒だし(>_<)

関節負荷の計算は、ある関節に対する各質点のモーメントを合計して出してみようかと思います。　ZMP導出のために各質点の位置と加速度は出ているので算出は可能。ただ、質点ｎの場合、ｎ×ｎ-1の計算をフレーム毎に行うことになり結構計算量は多くなりそう。　簡単な計算だけどね。

ちなみにラムダの多質点モデルは質点２５個所。　ZMP導出の際、多関節の四肢については先端から順に関節角度を適用していくから、足の場合、６関節だから　6!=720　腕の場合は４関節だから　4!=24　頭は3関節だから　3!=6　合計720*2+24*2+6=1494　1494回の回転座標計算を行う。　これをフレーム数行うので50フレームとして、74700回、1回補正をするとまた同じだけ計算するからその倍で149400回の行列計算(2×2の行列ですけど）　を行います。　質点が少ないからこれだけで済んでる。

これに比べると25×24×50=30000回だから大したことないか～、、　というわけには行かないか。　どんどん計算量増えていくと、ちとまずい。　なので、負荷計算は支持脚だけに適用しようかと思ってます。　採取したデータによると指示角度からずれているのは概ね支持脚の関節のみです。　

どちらにしても今のところは皮算用。　まずは補正値の作成からですが。。(^_^;)

各関節にかかるモーメントを計算しようと考えたわけです。

ZMP導出で計算している空間座標と空間加速度を使って、目的の関節座標と、ある質点の座標から関節から質点までの距離を求めて、同様に相対加速度を求めて、外積を取ればモーメントが求まる。　でも、関節はある１軸でしか回転しないわけだから3軸計算するのは無駄だねぇと思ったのですが、違いますね。　関節の姿勢を出して、関節の回転方向のモーメントを取り出さねばならないのかぁ。　思ったほど簡単じゃなかった。

ある関節の姿勢を算出するには3×3の姿勢行列の掛け算を関節段数回行わねばならないので、全関節についての計算を行おうとすると、ZMPの時より計算量増えてるやーん。

ってか、普通の会社員の日記の西さんがやろうとしている逆動力学計算をせねばならないということか。　これは大変そうだからもっと簡単にならんかなーと考えていたのだが、、、　とりあえず慣性テンソルは無視していいかなー。

とりあえずは補正データ作って動かしてみようかと思って、補正データを作り始めたのだけど、どうにもこうにもテンションが上がりません。でも、足のサーボ12サーボ分のデータを作って、作ったデータをサーボに読み込ませるプログラムを作って、傾向を見たらまた補正して、、、という作業を考えて、さらにはそのデータは実験だけで歩行パラメータが変わったら使えなくなるわけだし、、と考えると、1サーボ分作ったところで力尽きてしまいました。（ちなみに作成時間はほんの15分くらい）

本当はそういう地道な実験から有益なデータが生まれるのだろうけどねぇ。

うだうだしているわけにも行かないので、モーメントの計算をやってみることにしました。補正したい部分とモーメントが大きな部分が一致すればビンゴです。そのデータを使って補正データを生成できるかもしれません。

ZMPの導出では質点の位置・加速度しか算出していなかったのだけど、関節負荷を調べるには関節点の位置と加速度と姿勢を調べる必要があります。

姿勢は接続している関節での姿勢変更分だけ回転行列を掛けていって出来る3×3の行列で表します。　モーメントは負荷を計算する関節と質点の位置関係と加速度の関係からモーメントベクトルを算出します。　そして出来たモーメントベクトルと姿勢を表す回転行列の逆行列を掛け合わせることで、関節に対するモーメントベクトルを算出します。

関節の回転軸方向のベクトル成分がその関節の負荷となります。

姿勢を表す回転行列を行列のまま扱わなくてはならない上に逆行列も算出しなければならないので行列演算のクラスを作りました。

JOINT[1]は右足の股関節のロール軸です。　下のグラフが関節にかかるモーメントで、軸方向で有効なのがY軸成分です。　概ね補正が必要な部分が持ち上がっているように見えるのだけど、ピクンピクンとピークが出ています。　これは加速度の極性が変わる部分です。ZMPが切り替わる部分にあたります。

今の計算結果は、ある関節が質点から受けるモーメントを合計しているだけで地面に設置している概念がない。重力は計算しているので、目的の関節が空間に固定されているイメージか。　これでは遊脚と支持脚で受けるモーメントが変わることが表現できていないのでダメなのかもしれません。　グローバル座標を使ってるから大丈夫かと思ったんだけどな。

下のグラフは、上が右足の足首ロール軸、下が左足の足首ロール軸のモーメント。　有効なのはやはりY軸データです。

これはグラフの形が一緒なのだけど、値が全然違います。　上の右足の場合、値が小さい部分が支持脚になっている部分で、補正を掛けたいエリアです。　下のグラフだと値が大きな、部分で補正を掛けたいのです。

こんな感じなのだけど、もう少し整備すれば使えるかもしれないな。

支持脚が接地していることをどう表現すればいいのか、もんもんと考えてました。

片足で支持している時はその足を床に固定すれば、、　でも、両足で立っている時はどうなるの？　と考えていて、　そーか、ZMPから床反力が働くと考えばいいわけね。　と気付きました。　(^_^;)

早速、多質点から導出したZMPにロボットの総重量が原点に向かって力を与えるようなモデルにしてみました。

右足の足首ロール軸。　ずーん。。。　値が大きくなっただけって感じだ。　遊脚の時が一番モーメントが大きくなっちゃってます。

もうちょっとって気もするし、全然間違ってるのかもしれないし。　(>_<)　もちょっと検討だな。

各関節のモーメント算出の続き、、なんですが、注文しておいたNAS（１TB）が届いたので早速接続。

設定その他はすごく簡単に終わったのだけど、初回のバックアップはフルなので時間がかかる。　動作が重いPCで作業するのはイヤなので、随分と長い間放置している次期ラムダの大脳となる予定のC7/LinuxでのUVCインストールの続きをすることにしました。

バッファローのUVCカメラ。　うまくいったらもう一つ買ってステレオグラムに挑戦する予定。

UVCのインストール自体はごく簡単なのだけど、前回やったとき（確か年末？いや秋かな？）はUSBポートに接続してもドライバーがロードしなかった覚えがある。　そこからスタートです。　前回までの作業のメモを探したけど、ものっすごい簡単な落書きメモが残っているだけ。ダメですねぇ。

でも、やっているうちにだんだんと思い出してきて、作業を進めたのだけど、どうやらドライバーはロードしてビデオポートは有効になっている様子。

先人たちのブログなんかを参考にしながら fswebcam　をインストールしてみる。

なんか暗いし、走査線がずれてるけどそれなりに動作していることが確認できた。　ここまでくればなんとかなりそうな感じ。　次はカーネルリビルドしてカーネルをライトにシェイプアップするってのをやろう。

バックアップも終わったので関節モーメントの続きを開始。　ZMPに床反力を設定した時の計算結果はこれ。

算出できた結果は使い物にならない。　床反力を設定するだけじゃ遊脚の表現はできないですね。　支持脚はこの計算の仕方でもそんなにおかしな結果にはならないはずなのにダメです。なんか考え方が間違えているらしい。

足首ロール関節は、支持脚である間は計算上はほぼZMPと一致するから大きなモーメントを受けないはず。　対して、股関節はZMPからは空間的に離れているため、関節が受ける力も不均衡でモーメントが出てくるはず。　この計算じゃ股関節の方が値が小さくなってしまっている。　ちなみに実測だと足首ロールもオーバーシュートしてしまってるからモーメントが発生しているのだが、あれは股関節のオーバーシュートによってZMPがずれてしまい、足首にもモーメントが発生してしまっているのだと思っている。　つまり補正しなきゃならないのは股関節で、股関節の補正が成功すれば足首には補正は必要ない「はず」。

お風呂に浸かりながら考えたが、トラクションがかかった四肢先端とかかってない先端は取り扱いを変えないわけにはいかないですね。　そして、トラクションのかかり具合はZMPで決定するのが素直だろう。

UVCにしてもZMPにしても進展があるのかないのか、微妙だなぁ。

＃アイオーデータのNAS。　省電力機能で普段はHDDの回転を止めていて、アクセスがあったら回りだす。　でも、人為的なアクセスがなくても結構回りだす。　何がアクセスしているんだろう？？？？　DHCPサーバ、無線LAN-AP辺りかなと思うがDHCPサーバはとりあえず無実らしい。すると無線LAN-APか。

関節のモーメントについては、なんだかよく判らなくなってきたので一度頭から離してみようと思います。　考えてくるとモヤモヤしてしまって・・・、力学をもうちょっと勉強しないといけません。

なので、ZMP規範で生成した歩行データに補正を加える方を進めます。

いままでは姿勢値を更新していき、その姿勢値から関節角度を生成していましたが、あらかじめ用意した補正値列を加えるのにリアルに逆運動学計算するのもおかしな話です。　なので、ZMP計算で生成した角度列をストアしていき、そのデータを再生するようにしました。

このデータを再生する際に補正値をカップリングしていくような作りにします。

しかし、、、　計算が正しいのであれば、ラムダはZMP規範の歩行モーションでは歩けそうにない気がしてなりません。　とにかくものすごーーく左右に胴体を揺さぶるので、あれでバランスとれる気がしないです。

ラムダくらいのロボットの大きさだと歩行周期は0.2秒くらいにしなければ動的安定は得られないという話を聞いたことがありますが、それかなぁ。　1歩0.5秒じゃ無理かも。。

かといって0.2秒～0.3秒だと動作が速すぎてまともに動けない。　ちょっと絶望的な気持ちになってしまいました。

面白くない気分なので、ちょっと目先を変えて昨日のUVCの続きなぞやってみました。　昨日は静止画がかろうじて取得できましたが、今日は動画でモニターしてみます。

luvcviewというアプリがあって、先人たちの動作報告も「簡単に動いた」的なのが多い。　これをやってみると、　ホントに簡単に動いた。(^^ゞ

ただ、フレームレートが5fpsとかなんですね。　ちょっとしょぼいなー。　あと、カメラがCMOSだから露光時間が気になる。動作がある場合はどうなの？

決してべらぼうな速度でカメラを動かしたわけじゃないのだが流れちゃいますね。　これじゃ歩きながらの画像認識などは到底無理。

やっぱもっと値段の高いカメラじゃないとダメか。　分解してみるとレンズはピンホールカメラみたいな感じ。　広角レンズに変更もできなさそう。

フレームレートが低いのは光量が足りないからのようだ。LDCディスプレイのような明るい画面にすると、40fpsくらいまで一気に跳ね上がる。　そしてモーションブラーも随分とマシになる。　昼の屋外ならいいのだが、一般家庭の夜の屋内だと暗すぎるわけだな。　レンズも小さいし暗いわけだ。

CCDでUVCってのが見当たらない。　↓これみたいなレンズが大きいのならまだましか。（ミニレンズで交換できそうだし）

　無料貸し出しキャンペーンってなんだろ？　個人には売ってくれないのかな。　きっと高いんだろうけどね。

NTSCをUSBのUVCに変換するコンバータとかないのかな。ちっこくて安いの。

ラムダによるZMP規範での歩行生成。　左右にふっとぶのは、スタンスが広いからだろうということで、スタンスを100ミリから40ミリに狭めてデータを作成してみる。　40ミリで歩かせようとすると一部改造を要するのだが、四の五の言っていられない。

スタンスが広い歩行というのは、お相撲さんのシコのような体重移動をすべきで、上体を左右にスライドさせるような歩行には向かないような気がする。　スタンスが狭いと左右の動作幅が小さくなるはず。　あとは1歩の遷移時間を短くした方が、より動歩行に近づいて安定する「はず」。　

1歩が0.4秒でスタンスが40ミリ、1歩の歩幅が60ミリ（120ミリというべきか？）　これくらいで進めてみましょう。

昨日のうちに補正テーブルを読み込んで、歩行データとカップリングしながら実行できるプログラムはほぼ出来たので、あとは補正データを作るればよい。　それが大変なんだなー。

ちょっと歩かせてみたら、案の定足が干渉してしまう。でも、体重移動して遊脚を持ち上げるところまでを見る限り望みが見える！　よっしゃ、がんばるぞ。

昨日は関東組ロボット練習会に参加。　前の日に焼酎を飲んだので立ち上がりきつかったけど、そこは気力でカバー。

でも、行ったらえまのんさんが持ってきてたバルカンで遊んだりしてました。(^_^;)　（↓これね）

とはいえ、ずっと遊んでいたわけじゃなく、練習会には珍しくちゃんとデータ取りとかプログラム修正とか作業を進めまして、とうとうZMP歩行に補正データを加えて歩かせてみることに。

きっと股関節ロール軸の負荷オーバーが全ての問題だと決め付けて、まずは股関節ロール軸にだけ補正をくわえてみます。加えた補正はこんな感じ。

単に指示角度と採取角度の差を補正とするのではなく、オーバーシュートによってうまれたと思われる遅れについては補正を加えません。

そして歩かせて見ますと、、、やっぱり横に吹っ飛びます。　関節データを見ると、

オーバーシュートについては見事に補正されています。（黒く囲ってある部分）　ただし、大きく体を振って戻りの動作に入るはずが思い通りに戻りきっていない。（赤く囲ってある部分）

ちょっと設定を見直してみると、なんとトルクの設定値が80%だったので（膝は100%となっていたが）　これらの設定を100%に変更して再度データ取得からやりなおし。

淡い期待もあったのだが、結果は変わらなかった。ちょっとだけオーバーシュート幅が小さかったかな。　今度は振り戻しの部分にも補正をかけてみました。

おおぉぉ、良い感じ。　でも、歩けません。　まだオーバーシュートも残っています。　歩かせる様子を見た感じだと、振り戻しの時に、重心が（ZMPが）外側に行ってしまっている感じで上体が回転してしまっています。　どうやら足首が耐え切れないようです。

股関節ロールの補正値を見直す前に足首関節の様子を見てみましょう。

ところで、これらのデータは1歩0.4秒で足を揃えた状態から、左足→右足→左足　と前に送っていって足を揃えた状態になって停止する、という歩容なのですが、左足を前に送る時、つまり右足に体重を掛ける時だけ触れ幅が大きい。　オーバーシュートが大きくなってしまう原因かもしれません。

左足を前に送る場合は足が揃った状態から1歩前に出すケースと、右足が前に出ている状態から脚をそろえるケースの2タイプ、　右足を前に送る場合は左足が前に出ている状態から右足が前に出ている状態のケースなんですね。つまりロボットの重心点の移動速度は右足が前に出るケースの方が2倍速い訳です。

これはきっと考慮するに値するはず。　重心点の移動距離と1歩の遷移時間を関連付けた歩容生成を考えた方がいいかもしれません。　ほんとはそんなことしないでも重心点軌道を守ればいいはずなんですけどね。　恐らくなんだけど、ZMPと重心位置が一致する（加速度を考慮しない重心位置がZMPになる）状態は作っちゃいけない気がします。実は今の歩行パラメータもそこを意識しつつ、サーボの応答速度をも考慮して決めています。

さて、とは言いつつもやることがぶれるといいことはないでしょう。まずは足首ロール軸にも補正を加えてみましょう。

股関節ロール軸と足首ロール軸に補正を入れた状態での股関節ロール軸です↓

最初の1歩はまだオーバーシュートが残っています。補正値が足りないか。

逆にこちらは補正値が大きすぎる？最後の1歩は波形がくずれています。　足首については左足はアンダーになってしまった。補正値は不要であったか？

ちなみにまだ歩けません。　でも、横に吹っ飛ぶのはなくなったかも。　横じゃなくて後にこけるようになりました。今度はピッチ軸への補正を入れていきます。

今週は月曜日は仕事帰りに飲みに行くことになりぃ、火曜日は出張帰りに飲みながら帰りぃ、水曜日は単身赴任でこちらにやってきた後輩と飲みぃ、木曜日はやっぱり出張帰りにちょっと飲みに行くことになりぃで、毎日アルコールが入ってまして、ロボット開発はさっぱりできませんでした。　金曜日は色々あって飲んで帰りたい勢いだったんだけど元々酒に強くないのでさすがに飲まずに帰ってきました。(^^ゞ　火曜～木曜は二日酔いでした。　金曜日は、木曜日の飲みが「軽く」だったのでやっと体からアセトアルデヒドが抜けてくれました。　安心してくださいぴしぃさん、引きこもりニートじゃなくて飲んだくれサラリーマンですよ。(#^.^#)

先週の日曜日にもう一息で歩けるかってとこまで来て、ピッチ軸への補正でこりゃもう行けるんじゃない？と思ってたのだけど、ピッチ軸に補正を入れるとまた横に吹っ飛ぶようになりました。これが２２日の日誌更新後の話。

それはピッチ軸への補正が入ったことで力が後に流れることがなくなって、横への補正値に修正が必要になったんじゃなかろうかと思ってみました。

ちょっぴり補正値をいじった後に採取したデータがこれ↓　股関節ロール軸です。

随分と乱れてしまいました。　ちなみにこれは介添えなしでこけるに任せたデータなので有効なのは始めの方、横軸で13くらいまでです。

特徴的なのは横軸10くらいまではアンダーなのが、そこからオーバーに切り替わっている部分。　ピッチ軸への補正を入れたことで様相が随分とかわってしまったのかもしれません。　ちょこっとデータをいじってみたりしましたが改善のきざしなし。　抜本的に補正データを修正する必要があるようです。

今のデータはロール軸に補正を入れた後にピッチ軸に補正を入れたのだけど、逆のようですね。　ピッチ軸に補正を入れた後にロール軸に入れるのが正解かもしれません。あと、せっかくやり直すのだから歩幅に合わせた遷移時間の設定もしてみようかと思います。　あ、、明日ね。

ダッシュ２０００のところでラムダが自律で出走したおかげで取り上げられてます。　とうとうラムダもRobotWatchデビューです。　でもでも、マジではずかしーです。　７分かかってゴールしてませんから(>_<)　ゴール勝手に変えちゃいましたから(ーー;)　動画再生して、むずむずして途中で止めちゃいました。　早くちゃんと自律でダッシュ2000を完走しなきゃならないです。

普通の会社員の西さんがとうとう関節負荷の導出とそれを用いたフィードフォワード制御に成功したそうで、おめでとうございます。＼(~o~)／　全然日記が更新されないからきっとがんばっておられるんだろうなーとは思ってました。　いいなー、すごいなー。

うちの方は導出どころか補正をかけてもうまく歩けません。

まずは、ZMP計画を見直して、半歩のところは遷移時間を短くしてみました。

そして、ピッチ軸を補正してからロール軸を補正するようにしてみました。その結果がこれ↓

これでも横にこけちゃうんだよなー。　ちなみに最後の方はずれが大きかろうが小さかろうが、手で支えた状態だから気にしないでください。　気になるのは股関節ロール軸の初期部分の角度にずれがある。その後に目標角度に近づいているということは想定している加速度より大きな加速度になっているわけで、ZMPは外側にずれるから横にこけるという現象とも一致する。

さっきとうとうサーボケーブルが断線してしまって一気にテンションが下がってしまった。(ーー;)　もしかして、このサイズ・質量だと角加速度を考慮しないといけないってことはないだろうねぇ(^_^;)

いくら補正しても一向に歩けないのでとうとうテンションが下がってしまいました。

パラメータを色々変更して今のガラクタラムダでも歩けるパラメータを探したいところだけど、いちいち補正値を作成するのがめんどくさくて、、(>_<)　これじゃ、負荷算出ができたとしてもまともに歩けっこないじゃーん、って思えて余計にテンションがダウン。

動く様子を見てると、「もうちょっと前傾になれば、、」とか思うのだが、計算値そのままで歩けなきゃ意味が無い。　こういう時は歯がゆいですね。

色々愚痴りたいモードになってきたが、ここで愚痴っても事態は好転しないもんな。　ふんばってもうちょっとパラメータいじってみます。

気がつくと週末。　最近はウィークデーにロボット開発ができてません。

歩行パラメータを調整して、歩幅を小さくしたり、脚上げ高さを低くしてみたりした上で、補正値を加えてみても歩けません。　倒立振子で歩けていたのはなんだったんだろうなぁと不思議なくらいです。　もちろん今でも倒立振子ならそこそこ歩ける。

もちろん、導出式が間違えてたとか再生ルーチンにバグが、とかいうオチも有りなのですが、どうやら倒立振子での歩行より安定して歩かせたいと言う目論みはほぼ確実に無理な話のようです。

ということなので、やはりサーボの（ロボットの）パワーウェイトレシオが低いという問題を解消してみないと事の本質が見えてこないのではないかと思い到りました。

パワーウェイトレシオが低いロボットだと受動歩行的な歩容の方が有利ってのはもちろんそうです。倒立振子歩行は歩き始め以外は受動歩行に近いのでラムダでもなんとか歩けていたのかもしれません。

パワーの解決にはサーボの変更が必須。　次のサーボは、、フタバ派というわけじゃぁないんだけど、RS405CBを有力視しています。　プログラムも多分今のまま使えそうだし。　ところが発売時期が見えてこないので計画の立てようがない。待ってていいのか悪いのか、さっぱりアナウンスがないのも不安です。

結果はやっぱり「わからない」、サンプルだけでも入手して評価してみたかったのですが、それも成らず。　うぅ～、先に進まないなぁ。

多分問題ないとは思うのだが、多質点モデルからのZMP算出で、質点の角運動量の計算を省略しています。　教科書でも省略可能だって書いてありましたが、算出方法も判らないので、検討してみることにしてみます。　できるかどうかわかりませんが。

バイブル「ヒューマノイドロボット」の2.2.5回転行列の微分と角速度ベクトル　という章があります。　ここをお勉強。

読んでいくと「回転行列の転置行列が逆行列に等しい」と書いてます。　えっ　知らなかった。だったら大仰な逆行列算出をしなくてもいいんじゃん。でも、ほんとかなぁ、信じられない。回転行列って回転行列を掛け合わせた行列も回転行列なんだよなぁ。　行列と逆行列を掛けると単位行列になる。　つまり行列が回転行列なら、転置行列と掛けると単位行列になるってこと。　X軸の回転行列みたいな基本の回転行列ならわかるけど、複数軸を掛け合わせた回転行列でも成り立つのか試してみることに。



　Rxy = Rx・Ry =　

　 [1] [0] [0]

　 [0] [cx] [-sx]

　 [0] [sx] [cx]

　　　*

　 [cy] [0] [sy]

　 [0] [1] [0]

　 [-sy] [0] [cy]

　=

　 [cy] [0] [sy]

　 [sxsy] [cx] [-sxcy]

　 [-cxsy] [sx] [cxcy]





　t[Rxy] =

　　[cy] [sxsy] [-cxsy]

　　[0] [cx] [sx]

　　[sy] [-sxcy] [cxcy]





　t[Rxy] * Rxy =

　　[cycy+sxsysxsy+cxsycxsy] [sxsycx-cxsysx] [cysy-sxsysxcy-cxsycxcy]

　　[cxsxsy-sxcxsy] [cxcx+sxsx] [-cxsxcy+sxcxcy]

　　[sycy-sxcysxsy-cxcycxsy] [-sxcycx+cxcysx] [sysy+sxcysxcy+cxcycxcy]

　=

　　[1] [0] [0]

　　[0] [1] [0]

　　[0] [0] [1]

次は回転行列の微分です。　回転行列の微分と、回転行列の転置行列を掛け合わせたものが角速度ベクトルをひずみ対象行列に展開した行列になるらしい。回転行列の微分値は微小時間での回転行列の差分（回転行列的な差分）でいいのだろうか？

この回転行列的な差分というのが、ただの差じゃなくて、微小時間前の姿勢を基本とした時の現在の姿勢の回転行列のことで、回転行列と、微小時間前の回転行列の転置行列（逆行列）を掛けたものでいい「はず」なのだが、この回転行列の微分値と回転行列を掛けたものが必ずひずみ対象行列になるのだというのだ。　ひずみ対象行列ってのは転置行列を取ると、元の行列の符号を反転したものになるということで、つまりは単位行列では「1」になった要素がすべて「0」になるはずなのだ。　ややこしい。

各質点の姿勢を表す回転行列式は決まっているので、その回転行列の微分式も決まっている。でも、それを数式として保持しておくのは質点毎にあの回転行列のべらぼうな項数を微分するってことでべらべらぼうな数式長になってしまいます。　それをプログラムするにはハードコーディングならできるけどべた書きになってしまってダメダメだし、表現するデータ構造を考えてコーディングすることもちょっと考えられない。　すると、数値計算で行うことになる。

数学上代数表現ではそうなのだろうけど、微小時間が大きければもちろん誤差が大きくて「0」には成らないだろうし、0じゃなくても微小な値になればいいのかなぁ～。　これもやってみるしかないか。　むずかしいです。(>_<)

まだ諦めがつかなくて、歩行パラメータを変更して補正をかけたりしてみているのですが、どうも運動エネルギーが出すぎているように思える。　パラメータ調整していて、横に転ばなくなったら後に倒れ、後に倒れなくなったら横に転ぶ。　関節の角度軌道はほとんど計算どおりだし、、、　もちろん計算ミスかも知れないけれど。　(^^ゞ

そういうわけで、やっぱ角運動量も計算しないとまずいんじゃないかなぁという考えが大きくなってきた。というか、計算して影響の有無を確かめたい。ラムダは脚が重いので、もしかしたら無視できない差分があるのかもしれないと。。

で、昨日に引き続き質点の姿勢から角加速度を算出するお勉強なのですが、読み進むと2.2.7行列対数関数のところで回転行列から角速度ベクトルを算出する式が出てます。微小時間間隔での回転座標をこのまま数式に入れればいいのかな。わからん。　算出後に時間間隔で割ればいいのかな？

各フレームで質点の姿勢を計算することはできるので、フレーム間での回転行列が判ればいいはず。回転座標R(f-1)からR(f)　（fはフレーム）になる回転座標Rは

だと思っていたのに教科書には「二つ回転行列R1,R2の間の回転行列を求める　R = t[R1]R2　と書いてある。　ふむぅ～、間違えてたのかぁ？でも理解できない。　この教科書、間違いが多くて理解できないと「もしかして誤植？」と思ってしまう。　ちょっと試しに計算してみると↓



　R =　

　 [1] [0] [0]

　 [0] [cx] [-sx]

　 [0] [sx] [cx]

　R1 =　

　 [cy] [0] [sy]

　 [0] [1] [0]

　 [-sy] [0] [cy]

　R2 =

　 [cy] [0] [sy]

　 [sxsy] [cx] [-sxcy]

　 [-cxsy] [sx] [cxcy]





　R2 * t[R1] =

　　[cycy+sysy=1] [0] [-cysy+sycy=0]

　　[sxsycy-sxcysy=0] [cx] [-sxsysy-sxcycy=-sx]

　　[-cxsycy+cxcysy=0] [sx] [cxsysy+cxcycy=cx]



　t[R1] * R2 =

　　[cycy+sycxsy] [-sysx] [cysy-sycxcy]

　　[sxsy] [cx] [-sxcy]

　　[sycy-cycxsy] [cysx] [sysy+cycxcy]

やっぱそうだよね。自分の理解で合ってるらしい。　誤植？なのかな？　自信ないけど、プログラムは自分の理解で書いてみよう。

やってみました、角運動量を考慮した多質点モデルからのZMP導出。　結果的にはこれは計算しなくてもよさそうです。

前後方向は少し差が見えるけど、左右の動きにはほとんど差が無い。　予想に反する結果でした。　前後の重心位置が随分とずれているのは腕を後ろに伸ばすような姿勢にして、胴体を前のめりにするようにしてみたためです。　腕を下に下ろした姿勢だと、角運動量の考慮有り無しでほとんど計算結果が変わりませんでした。　こういうとこも影響するようです。　前後と左右、どちらも値が小さいカーブが角運動量を考慮した場合です。

コーディングして、エラーがでないようにしただけなので、プログラムが正しいかどうかもよくわからんのですけどね。

ZMP導出計算時に重ねて重心補正を加えるというくだらんバグを発見しました。　補正ループを3回回しても初期のオフセットが取れないのはおかしいなぁということでソースコード眺めていて気付きました。

昨日のグラフとは歩行姿勢が違うので単純比較できませんが、最初の部分のオフセットがすごーく小さくなりました。↓

これに補正をかけると、なんとかこけずにモーションを終えるところまできました。　今の歩容は、両足支持期間を持たない状態なので、もう少し工夫すればちゃんと歩けそうな雰囲気です。　ラムダががらくたなんじゃなくて、私のプログラムがヘタレなせいでしたよ。(>_<)　プログラムってかテキトーなこの性格のせいか。(^^ゞ

正直、もうラムダの機体じゃ歩けないんだと諦めていて、何が歩けなくなる決定打なのかを探っておしまいにしようと思っていたのだけど、それがどうも納得いく答えがでないので、計算ミスかバグが原因の可能性が消えない。そこで基本に立ち帰って細かくチェックしておりました。

色々と確認していくうちに、ラムダの大きさとか重さを考えると今までの計算結果がどうにも合点が行かなくなってきました。重心点の振幅が半分くらいじゃないとダメだろうと。

そして、やーっと本当のバグを見つけました。　下半身姿勢をサーボの角度に変換するときに、歩行動作の時にはしなくてもいいJ1Z変換を入れていたお陰で振幅が倍になってました。　なぜ倍になったかは、ソフトの作りの関係なので説明しませんが、そりゃー歩けないだろうと。　こないだ歩けそうだって言ってたのも実はスリップしていたせいだったのでパラメータ調整しても全然ダメだったんです。

そして、バグを修正して、補正値を入れた形でやっと歩くことができました。　西さんのプロトタイプみたいに華麗には行きませんけど。　まだ歩幅も小さいです。動画公開するほどの歩行じゃないんですけど、もうちょっと歩幅を増やすことができたら動画アップをしてみようかな。

でも、補正値作るのが結構大変です。　実用化するには補正値を計算生成せねばなりませんので関節負荷の計算が必須です。

自分のプログラムがバギーなのもそうですけど、ラムダがボロボロなのも本当です。　またサーボケーブルが切れかかっていて、ラムダを寝かせると接続が悪くなるらしい。　ビクビクと暴れます。(^_^;)　昨日まではラムダに嫌気がさしてたのでもうこのまま放置でシグマか、Linuxカーネルに作業移行しようとしてたところだったんですけどね。　もうちょっとラムダで遊べそうです。　関節負荷算出と、ZMP規範歩行の連続生成とかカーブ歩行とかのプログラミング楽しそうです。

一気に歩幅を大きくしたりしてみたが、歩けなかった。がっかり。。　

ZMP規範の歩行にしても、両足支持期間を相当設けなければ歩けない。　この辺りはロボット全体の剛性の低さが影響しているらしいのでロボットの機構が変わらなければ根本的な解決にはならないか。

下のグラフはバグを修正した時の目標ZMPに対する重心点軌道です。　以前掲載したグラフに比べて振幅が小さくなったのがわかると思います。

動作を見た感じよりも振幅が大きく感じます。　でも、この差は大きい。　左右の動作に関しては補正を加えなくても横に吹っ飛ぶことはなくなった。　データを見る限りやはりオーバーシュートはあるみたいだが。

膝関節などの撓みから計算より後に重心が偏ってしまって後にこける。　補正すると今度は遊脚が引っかかる感じ。　まだ試行錯誤が必要な感じだ。　大腿に設けたヨー軸が悪さをしている感じが無いでもない。

目標とするZMPのポイントを足裏のどこにおくかということや、上半身の姿勢なんかを調整して歩行を安定させようとしてみたが、どうやら無理な様子。

採取データを見るに、関節のガタ付きの影響で、目標角度からのズレの方向が急激に逆転したりすることが見て取れる。　トルク不足、速度不足もそうだが、ガタ付きがこれほど顕著に見えるとやっぱまずかろう。　補正データで歩かせるつもりもないのでこれはこれで終わりにして、関節負荷導出にとりかかることにする。

教科書読んで、やることは大体わかってきたのでプログラムを作って見るしかないのだけど、両足支持期間処理はどうすりゃいいのかなー。　特にZMPが両足の中間に無いケース。　

質点の空間速度と空間加速度を求める準備として、教科書201ページの「空間速度ベクトルによる剛体の運動方程式」を勉強しておりました。式6.20と式6.22を「空間慣性行列」を使ってまとめると式6.23になるらしいのだが、並進運動の方程式の方は変換できるのだが、回転運動の方程式が展開できない。うむむぅ～。

ウェッジ積というのが出てくるのだが、これがわからん。　普通のベクトルの外積のようなのだけど、ちょっと違うらしい。　気にせず進めたいところなのだが、こういうのが気になると手がつかなくなる性質で、うまく立ち回れない。　だめだなー。　

次のサーボなのだが、ふと4014でもいいかーと思ってしまって、そうすると機体を設計しないでもすぐ使える機体が色々ある。　メリッサのリンク脚「マーキュリー」が気になっていたのでこれにしちゃおうかと思って必要な部品をリストアップしたりしてました。　やはりでかいCPU積みたいし、余り動作は速くなくてもいいからちょっとでもペイロード稼げるようにしたいのだけど、4014でいいのかなーとか、サーボホーンはアルミのじゃなきゃならないのか？随分高いな、とか、ローハイトを買えばいいの？とか（ローハイトしかない？）、わからんことばっかです。　あと、マーキュリー買っても胴体は作らねばならない。どうすっかなー。　あ、あと4014と4014Sってのがあるみたいだけど、何が違うのだろう。

リンク脚だと上体の姿勢を変えるのに胴体に軸が必要になってしまうので避け続けてきたのだけど、歩行実験用にしようかなとか思ってその気になってたのだが、見積もるとえらい金額になってしまって、やっぱり迷ってしまいました。

逆に今のラムダの腕をはずせば軽くなるからサーボの動きもまともになるんじゃん、と思い、ラムダから首と腕を外して歩行の検討。

軽くなったら補正しなくても歩くようになったけど、なんかまだダメ。

どうもサーボの挙動がピーキーな感じ。ピクピクしているのがダメな感じです。　スムーズに動くようにパンチ・スロープ・マージンなどを調整。最後は胴体の重さを実際より重く設定してやると随分安定しだしました。　・・・・ちゃんと関節角度が指示値どおりになっているのかを確認せねば。　もし指示値どおりだったらまだバグ持ちだってことだな。ｶﾞｯｸﾘ

歩行実験だけならば、今のサーボでもできそうな感じ。　やはりRS601CRはトルクのわりに重過ぎるってことですね。機体重量１．５キロくらいがいいとこか。

重心点が目標ZMPを超えてしまう。　質点が広く分布してたらありえるのかな、とも思ったがありえないくらい横に吹っ飛んでしまう。　あらら、またバグかよ。と思ってバグ探しの旅に出ていたのだが、この3連休のうちのまるまる1日使っても何にも出てこない。　再現精度？モデル誤差？とか考えたが計算だけでこの結果だし、そういうレベルじゃない。　数式を眺めて色々考えたのだが、やはり質点の位置が影響しているのではないかという結論に至っております。

ラムダの脚は全部サーボで埋め尽くされているような形なので質量が脚全体に分布しています。　もちろんそれをリアルにモデル化してはいないのだけど、サーボの位置に質点を置くようにしています。

目標ZMPを目指す重心軌道を得るために許される運動が、重心高さ平面内での重心点の移動だけなので、質点の動きはそれにならった位置にしか動けません。なので、胴体の質量が軽いケースだと、質点の加速度を生むためには大きな動作が必要になってしまいます。

更には低い位置にある質点は加速度を生まない上に重心だけは下げるように働くため、目標ZMPを再現する重心点軌道を得るためには損しかありません。

どちらも試してみたら効果はありました。①は微調整可能なのですが、論理性が無い。　②は、計算上は床に接触して動作しない質点部分はロボットに含めなくとも構わないはずなのでアリかなと、、、　実際は足首サーボブロックの質量も無視しないとほぼ意味がないですが。。。

足首サーボブロックは実際にはわずかに運動するので計算に加えたいところではあるのですが、ここは目をつぶりましょう。

以上のことは理論的に正しいことを言ってるのかどうかわかりません。ただのバグかもしれませんので人に言いふらしたりしないようにお願いします。　(^^ゞ

ちなみに、教科書には「目標ZMPを再現する軌道は無限にある」という記述があったので、収束条件の問題かも知れないと思い、⊿tを小さくしてみたり、目標ZMPとの差分フィードバック量を小さくしたりしてみましたが、ほぼ同じ重心軌道になります。

バグかも知れないとは思いつつも、ラムダに手をつけた場合にはまともらしい計算結果を出すようになっていることからも、質量分布の問題じゃないかなーと思ってます。

サーボが変わって質量分布が変わったら比較実験できるのでその時までフリーズです。

※1　脚上げを行う場合は計算に加える必要があります。　脚上げの反動は結構な影響があるようです。

　お、これはぁ～どうなんかなーと思いつつ買って帰って読んだのだが、、なんかどっちつかずの話ばっかで面白くなかった。ハズレ。

そういや、深キョンのドロンジョ様も見に行ってないし、ハリウッド版ドラゴンボールはきっと観に行かないで終わっちゃいそう。ネタで観に行こうと思ったりしてたんだけどな。

マンガ読んでないで関節負荷計算のプログラムするんだった。　明日は横浜中華街で飲み会。

ラムダは頭と手が無い状態だし、土曜日の練習会に持っていくロボットが無いなぁ。　シグマは前回のわんだほープチ以来放置状態だし。相変らず煮詰まり状態。

今日は関東組ロボット練習会。　ラムダはガタガタだし、シグマを持っていくのは違うし、今やりたいのは関節負荷の導出プログラムのコーディングだし、ということでパソコンだけ持って参加。

結局、だべったりだべったりだべったりしてたのでパソコンさえも出さず仕舞い(^^ゞ

でも今日は、「普通の会社員の日記」の西さんが愛娘のあまねちゃんを連れてやってきたので関節負荷の算出についてちょこっとヒントをもらうことができました。がんばって追いつかねば。

あと、RS601CRは重くてトルクがないので、KRS4014で下半身を組むことを考えていたのだけど、RTに行ったらちょうどセール中。それもあと数日で終了ってことなので思い切って全身分のサーボを買っちゃいました。　現金のみなのでATMに走って金おろしてきましたよ。

アミエさんに「市販のフレーム使うなんてこと言わないでオリジナルのフレーム作ってくださいよー。」　と言われましたけど、帰ってからメリッサのマーキュリーを注文しちゃいました。(^・^)

既成のフレームで色んな楽しみ方が出来るってのはすごくいいことですよ。　その恩恵にあずかりたくなりました。　本来、自由度が低い平行リンク構造はラムダのコンセプトには合わないのだけど、マーキュリーはきれいにまとまっていて良いです。　同じコンセプトで設計したらマーキュリーの真似っこしか設計できないんじゃないかなと思いまして、ならば買っちゃえってとこです。時間の節約です。

最終的にはラムダマークⅡには平行じゃないリンク構造を考えているのでおいおいオリジナルフレームを考えていく所存です。

ううむぅ～、むずかしい。　アーム型ロボットであれば教科書どおりに計算すればいいのだが、２足歩行ロボットとなると両足支持時と片足支持時の扱いの違い具合が大きくてどのようにプログラムすればいいのかわからないぞ。

西さんとの会話ではZMPからの床反力を導入して胴体を基準に、、という路線だったのだが、やっぱり接地部を基準にせねばならないのではないかと思われる。

片足支持なら遊脚は末端になり、支持脚は土台になる。　今までの計算はすべて末端から胴体に向かって計算すればよかったので、腕や脚は胴体に、関節を経由してぶら下がっている形のデータ構造だったのだが、支持脚の場合は胴体は支持脚から生えているという扱いにせねばならないわけだ。

いや、、支持脚から胴体の空間速度と空間化速度を算出し、その後にその胴体の空間速度と空間加速度をベースに各脚の質点の空間速度及び空間加速度を求めればいいのかも？　そしてZMPに応じた荷重を足先に与えれば、、、。　相対的に考えればどっちをベースにしてもいいはずだよなぁ。　　　もう少し悩んでみます。。。

しかし、そんなに悩んでもいられない。あとひと月で「わんだほーぷち」である。　草加の時のシグマで出ればいいのかっていうとそれは余りに面白くないのでなんかしたいのだがあとひと月だもんな。

空間速度・空間加速度は根から末端に向かって計算していくのだが、支持脚を根とすると、支持脚と遊脚では計算する順序が逆転する。　また両足支持期間は両足とも根となる。　この時は負荷配分などは関係ないので淡々と計算すればいいわけだ。

支持脚を根として胴体に至り、胴体をジャンクションとして、腕とか頭などへ空間速度が累積されていくことになる。

両足支持の場合、どちらの脚も同じ胴体につながるわけだからどちらの脚から計算しても胴体の空間速度は同じになるはずだ。

で、計算してみたのだが、角速度は同じになるが、並進速度がちょっと違う。符号や大体の値の大きさは同じなのにちょっとだけ違うってのはなんなんだろうな。

今日中に全関節の空間速度と空間加速度まで終わらせるつもりだったのに最初っからつまずいてしまった。

昨日は突然の花見計画勃発。　寒そうだったのでダウンジャケット着込んで行ったのだが雨と寒さで中止。　普通にお店で飲んで帰ってきました。もう４月だってのに寒いなぁ。

飲んで帰ると、マーキュリーとサーボが届いてた。　思ったより早かったな。

空間速度の計算が合わないのはなんだろなーと仕事中も含めて考えていたのだが、結局おかしかったのはベクトル表現の問題でした。　空間速度の計算のために関節軸の方向を表す単位ベクトルを用意するのだが、ラムダの脚姿勢表現は右足を基準にしていて、左足はYZ平面対象のローカル座標を持っている。　同じIK関数を使えるようにするためにX軸は座標軸が反転、Y軸とZ軸は回転方向が反転しているという鏡面座標を使っていた。

この鏡面座標をグローバル座標に変換する際に、座標であればX座標値を反転するだけでよいのだが、ベクトルをグローバル座標に変換するにはX座標はそのままでY座標とZ座標を反転しなければならないのだった。　更には回転に関しては反転しなくて良いのだった。　この間違いのために左右の足で胴体の空間速度の計算結果が違ってしまっていたのでした。

晴れて、両足支持での計算で、それぞれの足を根元として股関節まで計算すると、胴体の空間速度は同じ値を取る様になりました。よかったよかった。(^。^)

デバッグの合間にマーキュリーを仮組みしたりしていたのだが、これって股関節ヨー軸は直交してないんですね。　リンク足だからいいけどそうじゃなきゃヤコビアンの出番になるところでした。(>_<)　　マーキュリー（リンク足）ってサーボを７個も使って自由度は５しかない、更には腰を沈ませた姿勢では自由度は極端に狭くなるので重心補正なんかも効かなくなる。　わかっちゃいたけど、用途限定の構造ですねー。　ピッチ角をつぶして得たペイロードに期待しているのだがどんなものなのだろうか。

ところで4000番サーボって始めて使うのだが、「原点調整」ってサーボのニュートラル位置出しってことでよろしいのでしょうか？

今日は久しぶりにシグマに通電してみました。　シグマは「わんだほーぷち」しか出られないからここはやっぱりやっぱシグマかなぁと思いまして。。　　と、思ったら夢思考にラムダロボット研究所が多足の話でリンクされてました。　ランダムロボット研究所　(^。^)　フフフ　　もやねさんとか西さんと並べられるのは恥ずかしいですよ。

マーキュリーやらサーボが届いたので、ラムダマークⅡの設計をしながら関節負荷の導出プログラムを続けてます。　マーキュリー脚を組み立てたりもしてるのでプログラムは3割くらいか？

例によって計算が合ってるかどうかわかりませんが、空間速度・空間加速度までは合ってるっぽかったですね。

このグラフを見ても、動作と負荷がリンクしているように見えるのですが、両足支持期間の処理がまだうまく行ってないようです。

両足支持期間の処理はいまのところ手抜きでして、支持している両足共に片足支持と同じように上体の全負荷を与えています。

右膝の計算はうまく行ってるように見えるのだけど、左膝の両足支持期間はおかしいなー。負荷が小さすぎ。全部右膝で持ってるようになっているらしい。ふむ。。。。

半ば意地になってプログラムしているようなものなのですが、マーキュリー脚だと負荷に強いので関節負荷による補正は必要ないかもしれない。　何よりリンク脚の負荷は同じように計算できるのだろうか。(^_^;)

マーキュリー脚を組み立てました。　ガタがなくっていいですね。でもやっぱり自由度が低くて、ラムダでやりたい動作はすべてできないので実験用（またはその他のロボット活動用？）ですね。

腰の姿勢は床面と平行にしかならないので胴体にも軸が必要になります。ここがガタ付いたら意味ないんだけどなぁ。

下半身ラムダとマーキュリー脚を並べてみる。ラムダが膝曲げ姿勢であることを加味しても足長だ。　601サーボの扁平な形で形成されたラムダの脚を見慣れているせいか、細い足だな。　でも、筋骨隆々なんだよね。

さて、腰周りを設計して動くようにせねば。　SEMB1200Aで動かすので実際に動き出すまでまだしばらくかかります。　4000番サーボ使って、イトーレーネツのブラケット類を使うと大体組み立ってしまうんですね。精度がいいし、ブラケットは買っちゃおうかなー。

風呂から上がってデバッグした結果、両足支持期の各関節の負荷が同じになりました。右股ロールなどでピクンと飛び出しているのは遊脚が上がる瞬間です。

足首ピッチの異常な立ち上がりがわからんですね。ZMPが抜けた瞬間で、負荷がなくなる瞬間のはずなのだが。