もうすぐこうして物を拾って持ってこれるようになる。（と、いいなぁ）■９月１日■
昨日は打ち合わせのあと少し時間があったので会社の図書館に本を探しに行って来た。
主に人工知能について、いい本がないかと思って探したのだがフィーリングに合う本は見つからなかった。
大体にして気に入らないのが人工知能の専門書や解説書にはすぐに「コンピュータに感情を持たせることができるか」的な話がテーマになる。純粋な学術書ではそこまで俗ではないにしてもすぐに高度な精神活動の可不可の判定をしようとする。もし感情やら発明やらが大脳の機械的動作なのであれば機械で実現できるのは明白。今日できるかできないか、今の技術でできるかできないかは別問題。機械的動作なのかどうかの証明が必要ならばそちらをがんばるべきだろう。 なんだろうか、歯車の設計さえ出来ない人間に核融合プラントの設計が出来ないのであなたは人間じゃありませんって言ってるような感じ？知能・知識・学習といった単語の定義もちょっとはずしてるような気もする。
自分が開発しているロボットも含めて、ロボットの機械的制御ができても人工知能がなければ「ロボット」とは言えない。出来れば「大脳」で活動すべきだし、少なくとも「小脳」で動かなきゃならない。歩行プログラムなど（今の段階だとまだ未熟すぎるのだが）は言えば小脳活動と言えなくはない。PCでいえばデバイスドライバーみたいな位置づけかと思う。動作に関して一揃い整えば小脳は完成といっても良い。本当は動作を取得すべきだろうし、環境に応じて修正ができなければならない。そういう意味ではまだ人工知能としては小脳の運動学習機能さえにも到っていないのではなかろうか。感情の実装について語るのは時期尚早過ぎというものだろう。
結局、図書館では
①ロボットの運動学
②マルチボディダイナミクス
③独創的ロボットの研究開発
という３冊を借りてきた。①②は内容が似通っている。同類の本もいっぱい家にある。③はいろんな面白いロボットの開発事例紹介が載っている。
今までの段階では線形倒立振子の一般解を使って動作を生成することができたが、全身運動になると全身の関節の動きによって作られる運動を解く必要がある。それも質点ではなく剛体の運動として解く必要がでてくるんじゃないかなと思っている。実際、倒立振子（「もどき」だが）で歩かせているが、重心点を質点と見なした計算しかしていない。なので重心点（というかロボット全体）に発生するモーメントは無視している。全身運動でモーメントをキャンセルするような動きを発生させればもっと安定させることが出来るはずだと思う。そういったことをやるためには自分の物理学力と数学力が圧倒的に足りないのだ。手取り足取り丁寧に数学と物理を教えてくれる友達がホシイ。。
③についてはブログの方に書きます。
■９月２日■
昨日から今日の二日間、とてもがんばった。
上半身と下半身を接続し、不定の姿勢からトルクを入れてロボットが自分で姿勢を把握できるようにしたのだが、随分と手間がかかってしまった。
歩行のプログラムのために足の姿勢は地面接地部を管理しているのだが、不定の姿勢からそれをやるのが結構大変で時間を食った。次に上半身との連携のために上半身に下半身の動きの情報を渡すのだが、やはり歩行プログラムを見やすくするためにさまざまな変数を設定してるため、クリアに渡せる変数がそろって無い。
仕方が無いので歩行プログラムのために作った変数を整理したり展開して単純な数値に置き換える関数を作ったりで二日間が終わってしまった。でも、なんとか動きすべてについて展開できたので上半身に渡せるようになったはず。（まだ渡すためのコーディングをしていない）
まだできていないのが重心補正の管理（ってのかな？）。基本的にほぼすべての姿勢を数値に変換できるようになったのだが、動作を開始した１フレーム目は重心補正が大きく加わるためビクッっと動いてしまう。これが許せないのでもうすこし工夫が要る。
もうすぐこうして物を拾って持ってこれるようになる。（と、いいなぁ）
物を持って歩くときはX-Z平面を拘束してY軸はフリー。このケースが一番多いのかな。あとはボールを投げるとか上半身と下半身の動きを同期させて大きな動きを生むようなケース。下半身の動きがY軸がメインだからY軸が特別扱いになる場合が圧倒的に多くなる。
階段を昇るときは手すりを持たせて昇るってのを計画してるからY軸フリーだけじゃだめで任意軸がフリーにできるようにしないといかんのかな？
不定姿勢からの動作をすべて逆運動学計算で動かしているのだが、ここにも課題が多く残っている。
要は作りこみが足りないって話なのだが、逆運動計算で一番簡単に動かすのは空間の最短距離をリニアに移動すること。今はこれで動かしている。そのため、途中経過でロボットには有り得ない姿勢を通過するケースが多々発生する。歩行なんかだと動きが限定されているので基本的に問題にならない。
足はまぁ、立ち上がるためのプロセスが必要で、どうしてもある程度のポーズを経由することになるだろうからそれほど問題じゃない。 問題は腕の方で、用途がさまざまなので動く範囲はすべて動く可能性がある、というか動かせるような仕組みにしておく必要がある。最低でも体や動いている足との干渉は管理しなければならない。すると空間を直線で移動するって話はダメで、ルートを考える必要があるのだ。どうやるんだろう？ あとは肘の角度の問題など、無限に解がある逆運動学のどのケースを採用するかを限定する仕組みなんかが必要。
まだまだその必要性には到っていないのでじっくりと検討して行こう。
■９月６日■
不定姿勢からの逆運動学制御。
トルクを入れた時点での重心補正はトルクを入れる前に実関節角度で重心を計算して補正を入れることでクリア。重心計算はリンクごとの重心位置とリンク位置をリストで持っていて、順に合成していく。合成はオペレータを使うので、プログラム上の記述では
a = a * b;
という記述で行えるようにしている。 関節の指示値を使うか実測値を使うかをパラメータで与えたいのだが、オペレータだから引数を持たせられないし、関数型に書き換えるのはイヤだし、ということでstatic属性のclassメンバーを作って、それをスイッチにすることで全インスタンスに指示できる。特に高度なプログラム記述を行っているわけではないがC言語を使ったあとにC++を使うと色々な点で便利だなと思う。その便利さが、プログラムのサイズとか処理にどれくらい負担をかけているのかは知らないけど。
いまは開発段階だから、複雑でスリムより読みやすさ書きやすさを優先すべきだろう。今でも十分複雑なコーディングだし。
重心計算を、関節の指示値計算と実測値計算で切り替えられるようにしてから動作させてみると、なんだか動きがギクシャクしている。おかしいな？
調べるとサーボに対する角度コマンドが１６サーボ以上に出されている場合にコマンドが実行されていない感じだ。腕の動きを止めたり、片足を止めたりすると正常に動く。
確か日曜日にも同じ動作をさせたが、正常に動いていたはず？？？重心計算の違いはサーボには関係ないし？？ さっぱり狐につままれた気持ちで色々テストしたが、やはりマルチサーボコマンド（ロングコマンド）で１６サーボとか１８サーボ動かすとおかしくなる。 なのにサーボテストコマンドで２０サーボ同時動作は正常に見える。
いまのプログラムは25ms毎に角度コマンドを出している。目標角度までの移動時間は０としているが、毎回、IDと角度と時間で５byteのデータを送っている。正常動作する１５サーボの場合はヘッダーを入れて７５byte。 異常となる１６サーボだと８０byte。この間にエラーを起こす問題があるらしい。通信レートは460800bpsなのでこの送信には2.5msほどしかかからないはず。ためしに時間を0に固定して角度データだけを送った場合は１６サーボ以上でも正常に動作する。一度に送るデータ量の差が問題になっているのだ。
実はこのあたりで、RPU-100をリブートすれば直るかもなーと思えてきたのだが、原因究明のためにもうしばらく試したが改善することはなかった。（原因もわからなかった）
サーボテストコマンドは小さな角度変化を全サーボに一斉に与えているが、おかしな動作となる屈伸動作は角度変化量が大きい。もしかしたら電流量の差で通信に異常が出ているのかも。
結局、一度RPU-100の電源を落として冷やしてから再度試すと正常に動いた。RPU-100のハードのブートエラーなのか、熱暴走によるものなのか、ケーブルの電源ラインが貧弱なせいなのかは不明のまま。
見かけ上は正常に動作するために不正動作を早期に見つけるのが困難。起動時にサーボ動作テストをしても熱による動作不全なら急に発生するわけだし、今のところ目視でしかわからない。しばらく保留だが、なんらかのシステム監視の仕組みを考えなければならない。真因究明に時間を取られるのはいやなので状況監視としましょう。
上半身を動かすようになってから、なにか意味のある動作をさせたいと思って考えている。腕の動作は足よりも難しい上に腕の自由度が少なくて出来る事も大してないのだ。
やはり、今の状況に一番即していて、意味のある動作というと、「歩行の安定化のための上体の動作を生成する」ということだろうか。 立ち上がりとかは今のところ静的な動作だから別にして。
対策すべきは歩行時の旋回（Z軸回転）と転倒（X軸回転）の抑止。 ちょっと整理してみる。
①何がやりたいか 歩行時の安定化。具体的にはZ軸回転とX軸回転を上半身の動作にて抑制したい。
いま、ラムダは重心点を質点とみなして倒立振子と仮想し、質点の動きを支持脚（と一部遊脚）で生成することで歩かせている。実際には質点ではなく大きさを持った物体でありさらには剛体にも程遠い。ロボット自体の姿勢は路面とグリップすることで矯正され、ある程度は補償されるが、グリップや関節のインピーダンスの関係で回転が生じる。
この回転に相当する角運動量を上体の動きで発生させ、相殺することで動作の安定を図りたい。
②歩行動作によって生じる角運動量を知る。
回転の原因はもちろん質点ではなく大きさをもったロボット自体の運動として発生する回転。その他、遊脚の動きは一貫して倒立振子動作とは違う動きをしており、角加速度を生む原因となっているだろう。
実際の腰部分の動きは腰部分の角速度センサーにて知ることができるが、検出される角速度は支持脚のグリップにより相殺されたあと、足の各関節のインピーダンスによって回転動作したものであり、運動量全体ではない。消極的な対策としては腰部分の角速度がゼロとなるような上体動作を発生させるという手段も考えられる。
どれほどの精度が得られるかの問題があるが、全身の姿勢によって得られる形状情報から発生する運動量を計算すべきだと思っている。まだよくわかっていないのだが、Z軸回転は、正確には支持脚の接地点と重心点を通る線を軸とした回転。物性値として、その軸回りでの慣性モーメントを求めなければならなそう。
転倒方向の回転については、接地点を中心とした慣性モーメントか、重心点を中心とした慣性モーメントを求めるべきなのかよくわかっていない。
現在の姿勢の他に各関節のインピーダンスの問題があるだろう。先にも書いたがロボットが剛体ならば（もしくはインピーダンスが十分に高ければ）グリップがあるうちは（モーメントは発生するが）角速度は発生しない。
関節のインピーダンスが低いために位相遅れを起こしながら角速度が発生してしまう。
これらを物理的に解くにはインピーダンスの測定という問題もあるが、偏微分方程式をたてなければならない（はず）。万が一式が立てられても解けないし、多分数値解析しかできないから計算に時間がかかりすぎる。
といった感じで問題がどんどんと困難な方へ大きくなっていくのでどっかでストップせねば。
とりあえずはもう少し検討した上で、慣性モーメントの算出や物体として発生する角運動エネルギーを計算してみることを試みようと思う。
慣性モーメントの算出は重心計算と一緒で各リンクを質点と見なして計算しようと考えているが、胴体は質量が大きな上にサーボが２つ、RPU-100がひとつ、と質量が分布している。さらにはロボットの中心に位置しているので、質点で計算すると慣性モーメントの計算にはほとんど寄与しない。ここはサーボとRPU-100を質点として分離し、慣性モーメントへの寄与をさせないと誤差が大きいかなと思う。
やらなければならないことは山のようにあるが、一つ一つ片付けていくしかない。
とりあえず、下半身が動いても手先の座標が変わらないようになったので動画を掲載しておく。動画あげておく程のものじゃないのだが。。
いまいちはっきりしないが手に持ってるアヒルちゃんの動きを比べると同期させているほうが幾分動きが小さいのがわかる。関節のインピーダンスや床面との接触の関係で実際には関節角度以上の動きをしている（それも位相遅れをしつつ）ので計算でわかる分だけをキャンセルしても足りない。これ以上を求めると偏微分方程式との戦いか、フィードバック周期を短くした角速度センサーによる姿勢フィードバックか、フィードフォワード制御をするということになるだろう。方向としては動作を学習し、フィードフォワードをかけるという方向に進めて行きたい。
プログラムをデバッグしている段階で、ロボットを歩かせてみたところ、さっぱり歩けなくなっていた。えぇぇぇ～、なんでーー。ということであせって歩行をデバッグしていた。下半身姿勢のパラメータを整理した時の配慮漏れなどがあって思い通りの動きをさせていなかったのが原因。あとはあちこちがギシギシ言い出していたので関節のネジを増し締め。ギシギシ言うときはネジが緩んでいる訳じゃなくてフレームにネジやサーボの軸が食い込んでしまっているような感じらしい。一度緩めてから締めなおすとギシギシ言わなくなる。ただ増し締めしてもギシギシは直らない。
フレームの整備とプログラムの修正をすると無事歩くようになった。驚くことに色々なパラメータ計算を加えたことで、いつのまにか後退ができるようになっていた。ラムダは重心のバランスのせいか、後退がダメで、そのうち検討しようと思ってそのままにしておいた。今日、デバッグする際、コントローラのトグルが後退になっているのに気付かず歩かせるとスタスタと後ろに歩き出したのだ。原因は重心補正が効いてるのかなと思います。剛性を上げるとか、重量を下げるとか重臣を下げるという目的でフレーム変更をしたのだけど、前後の重心バランスは取れてない。というかバッテリーを乗せずに検討しているのでバッテリー分軽いし、重心もずれている。
いま持たせているのは軽ーいモノだけど、持てる範囲で重いモノを持ったとしても、その質量が判れば重心計算に加えることでバランスを取って歩くことが出来るようになるはず。今のラムダではセンサーが足りないけど、そっと持ち上げて重量を想定するとか、画像認識からそれが何なのかを知識ベースから情報を得て重量を想定するとかそういう感じにしたい。
上半身を結合したことでロボットらしいアイテムが現実味を帯びてきた感じ。 まぁ、実現するかどうかってのは別問題なんだけどね。
■９月９日■
全身動作を考えると難しいことばかり。 特に腕や足の動作範囲や特異姿勢を考えるとどのような仕組みで管理し、動作計画を立てればいいのかさっぱり思いつかない。
本当は腕が体にぶつかったり関節が曲がらないことを学習して徐々に動作が洗練されるってのがいいのだろうけど、そういうシステムを構築するための情報が足りない。まだまだ積み重ねが必要だろう。なによりも今のラムダだとセンサーが足りないからデータとして保持してデータと照合してぶつかるとか曲がらないとかってことを判断するしかない。
動作生成にしてもそう。思考錯誤によって動きを獲得するにはまだ仕組みが低レベル過ぎる。足を伸ばしたらどうなるこうなる。重心を崩したらどうなるってのをデータとして与えなければ獲得できないし、最適化できない。今はまだ基本動作を数値的に組み立ててロボットの人工知能に任せられるほどレベル（レイヤー）を上げることに専念するしかないだろう。
で、何が必要なのか、何が問題になるのかさえもわからないので、黙々と検討するのをやめて、ある意味実験として上半身と下半身の協調動作をさせてみようと思い、４足歩行動作を組み立ててみた。
ちなみにこの姿勢で倒立振子基調の２足歩行動作を行うとものすごい前後動作をしながら歩く。そうか。重心がものすごく前に倒れてるんだからそうなるんだー。ちょっといじればその関数を使うこともできたのだけど別に足運び動作関数を作りました。
動作は下半身基調で、左右のスイングや足を運ぶことにより前に進むという動作は下半身モデルで生成。ただし、ロボットの関節構造上、足のスタンスには制限があり、股関節の幅以上には足を広げられない。
上半身は下半身の動きに従属する形で動作する。ただし、進んで行くと腕がおいてけぼりになってしまうのでいいタイミングで腕を前に送る動作が必要。これは上半身モデルで生成。足を送る動作と腕を送る動作はクローラ歩行の歩容のタイミング。
手も足もちゃんと持ち上げて運んでいるのだけど、うまく重心移動できずにすり足（すり手）になっている。まぁ、ここは深追いしない。
今はメインループ（定時割り込み処理）内でベタ書きして実現しているが、上半身と下半身動作を協調動作させるために必要な仕組みや問題はほの見えてきた。でも、しばらくはいろんなタイプの動作をベタ書きし続けるしかないだろうな。この４足歩行にしても、手を順繰りに送るのも腕の動作範囲を出てしまうからだし、そのままだとこけてしまうからであり、必然の動作として動作を発生させることが出来るはず。（たとえば重心補正のように）
そうしたことを察知してどのように振舞うべきなのかを自分で構築できるようにすれば、本当にキモの部分だけを記述すれば（もしくは教えれば）複雑な動作を構築できるはず。
４足歩行なんかはそれぞれの足を運ぶタイミングが重要で、歩行のように基本動作にするしかないけど、手だけ動かしても足だけ動かしても有効。手を止めて足だけを歩かせると、上の動画の後半の「足寄せ」になる。まだ足しか動かしていないが、重心の移動も同時にやれば胴体重心を両足の支持多角形内に入れるところまで移動できそう。
手だけをヨチヨチさせても良い。その方がスマートな動作かも知れない。
もうすぐ４つんばいから立ち上がるモーションを生成できるようになるかな。 四つん這いからの立ち上がり動作が一番簡単なんだけどね。体育座りから立ち上がるのは難しそう。
まだまだ上半身動作は動作不審なところがあるんだけど、頭部のメカ設計もしなきゃな。のんびりしてると冬に突入しちゃいそう。冬の工作は真夏の工作以上にきついので避けたい。
そういや、なんとなく、目に付いたので「パトレイバー」を読み返してる。イングラムもグリフォンもかっこいいけど、一番かっこいいなと思うのは自衛隊のＸ－１０（漫画版）。 イングラムもグリフォンもアンテナみたいなのとか角がある。Ｘ－１０は角がないところがいい。 ・・・ちょっと話が変わるけど、全身の動作を考えると外装が欲しいなと思えてくる。アルミ部品とかケーブル剥き出しだとひっかかってしまってうまくない。足同士や腕と体が干渉しても外装がツルリとしてたらひっかかったりはしないから異常事態に陥りにくいはず。Ｘ－１０がいいなって思う感覚はそうしたところとつながってるんだろうな。
■９月１０日■
四つん這いも出来たし、次は何をすりゃいいんだっけ？
四つん這いはもう少しおいといて、上半身－下半身協調動作の検討を進めてインプリメントの形を考える。四つん這いをさせるわけじゃなく、立ち上がり動作などのための基本動作の一部として実装する感じ。
全身動作での、特に腕の動作での自分自身や床面との干渉についてはどうやって計算させるかをちょっと考えたが、なんてことはない。 体の３次元データを持って、交差を判定するのが簡単だろう。ロボットの体をポリゴンで包んで、腕や足の動作に関して、ある点がポリゴンと交差するかを検査するしかない。移動フレームすべてを計算するのは大変なので、１０フレーム毎に検査するとかになるかな。もちろん動作する前に検査して、その結果をもって動作軌道の決定に利用すると。
あと、上半身の動きで歩行の安定化を図る検討は、遊脚の復帰動作によって生じる加速度でモーメントが生じるという前提で考えて、上半身を動かすことでそのモーメントを消去する。 というのを考えた。腰のひねりで角加速度を発生させてトルクを消去するから上半身は関係ない。 いうなら腕の姿勢で慣性モーメントが変わるから慣性モーメントを大きくすれば小さな角加速度で済むって話かな。協調動作じゃないな。。。
不定姿勢からの処理と上半身・下半身の連携が随分進んだので、ひさしぶりに転倒回避歩行のコードを眺めていた。 転倒しかかった姿勢を数値化し、その状態からの歩行を開始するということだったのだが、その姿勢変換が問題で止まっていた。
考えると不定姿勢の件でいろんな胴体姿勢を持つ下半身姿勢の構築ができるようになっていたので、悩んでいた部分が簡単に出来てしまった。 まだデバッグしてないけど。
進んだのでまた転倒回避歩行をやろうかな。 あ、頭部の設計しなきゃならないんだっけ。
昨日、Ｘ－１０って書いたのはヘルダイバーっていう名前が付いてるらしい。コミック版でもついてたっけ？プラモデルも出てたみたいだけど、コミックスに出てる99式とちょっとプロポーションが違う。コミックスのがいいのだ。
■９月１1日■
転倒回避歩行の開始姿勢の変換はできたので、次に動いている時の姿勢取得について確認。
この日誌を見た方にアドバイスをもらいまして、ロボットが動くことでの加速度分を補正する必要があるということでそれをやってみることに。
加速度センサーは主に重力加速度を捉えているが、センサーが運動している時はその運動に関する加速度ももちろんセンスする。つまり、ロボットが倒れているときはセンサーも動いているため、その値は重力加速度のみではなくなっている。
↓ロボットを自由落下させた時のＺ軸方向の加速度センサー値
自由落下と言ってもガシャンと落としたわけじゃなくそれくらいの速度で下に移動させた感じ。移動によって生じた加速度分が目減りしているのがわかる。
ロボットが転倒する時の姿勢は主にＹ軸の加速度センサーで取得していたが、転倒軌道の接線になるのでもろに転倒加速度の影響を受ける。逆にＺ軸センサーは転倒に関しては転倒軌道の法線方向となるため、転倒加速度の影響は受けない。ただ、転倒を開始するあたりは分解能が悪いため、使っていなかった。
↑水色っぽいのがＺ軸加速度。紺色っぽいのがＹ軸加速度。赤がＸ軸回りの角速度。
Ｙ軸は転倒に合わせて急激に立ち上がるはずが、移動加速度分目減りしてしまっている。転倒し終わって移動速度の変化が収まると重力加速度のみを感知するようになる。
角速度は時間とともに増加する。床面に激突するところで急激に減速している。角速度のピークとＹ軸の加速度が小さな値になってしまっている部分が一致している。グラフの形としてはそのようにリンクしている。
だが、、角速度から角加速度を求めて、並進加速度に変換して加速度センサー値に加えても、角速度センサーの値が小さすぎて補正にならない。うーん。。なんか間違えてるんだろうなぁ。
とにかく、ロボットが動いているときは姿勢取得にはＹ軸加速度センサー値はそのまま使えないのははっきりした。Ｚ軸加速度センサーを使った方がましかもなぁ～。
■９月１４日■
色々と試したが、加速度センサーの値をジャイロデータで補償する方法はダメでした。
動いている時、というか転倒する時の姿勢角度はジャイロデータを積分して得るのが一番よさそう。ジャイロデータにはどうしてもオフセットがあるため、適切な時に積分を開始しなければならない。相対的な姿勢角度しか得られないので、静止時の姿勢角度を加速度センサーで得て、動き出す時に積分を開始する、ということが必要となる。
Ｙ軸ジャイロ積分2というのはサンプリング周期が25msと長いのでその間を直線補完してみたもの。加速度が大きくなると誤差が大きくなるのでちょっと効いてくる。
Ｚ軸加速度センサーは直立付近の分解能が悪すぎて使えない。こないだは転倒速度の影響は受けないと書いたが、遠心力の影響は受けるので正確には転倒速度の影響は受けるはず。速度が速くなる部分では誤差が大きいはず。Ｚ軸加速度センサーがある程度信用できる45度付近はジャイロと加速度センサーの値はほぼ一致しているのでなかなか良い結果と言えるだろう。
さて、静止時と動作時をうまく切り替えて転倒する際の姿勢角度を取得できるものなのだろうか。。。
あと、転倒回避歩行を行う際に線形倒立振子として振舞って動くから、転倒することを計算しなくて良い「はず」と考えて処理していたのだが、実際は減速をしながら足を出すようにしているので転倒しながらの動作となるはず。足を出す0.3秒～0.4秒の間にどれくらい転倒するのかを計算してみることに。
転倒の計算は逐次数値計算をしなければならないので、実際にコマ送りで速度と位置を計算しなければならない。コマ送りの間隔が小さい程正確に計算できる。
初速：300mm/sで倒すと、大体0.3秒で完全に倒れる。また、高さが140mmになるまでが0.24秒ということだった。ドットは0.01秒毎の質点の位置。
そういえば、歩行モーションの生成では単純倒立振子動作をさせているが、軌道生成は線形倒立振子を使っている。この差はどのくらいなのか、差を計算したことがなかったので計算してみることに。
遷移時間を0.4秒としたとき、初速は約400mm/sとなる。0.4秒後の位置は10㎜弱の違い。うーん。あまり致命的な違いにはならないという結論でいいのかな？dtが0.01と大きいので誤差が大きいのかも知れない。
この値を信用すると、同じ初速の時に単純倒立振子の方が振れが小さいわけだ。計算は線形でやっているとなると、少しオーバーシュート気味の動作となるのはこのせいなのか？
どちらにしても遷移時間を指定して軌道を求めるといった使い方ができないので、歩行軌道生成には使いにくい。 転倒時の軌道計算は初速が決まっているわけだからループを少しまわすだけでいいので使えそうだ。ざっくり試してみるとdtは0.02秒以下にしなければ使い物にならなさそう。初速にいろんな値を入れてみたが、転倒判定に線形倒立振子の一般解を使うことは大きな問題はなさそう。単純倒立振子なら転倒しないが、線形なら転倒してしまうというケースはありそうだが、検出できないケースよりはマシという考え。
検出してから転倒回避歩行が始まるまでのタイムラグがいくらかある。その間に移動する量を計算した方がいいかな。
＃ちなみにその後、dtを0.001秒にして計算してみたけどあんまり変化はありませんでした。
■９月１６日■
動作時はジャイロデータで、静止時は加速度センサーで姿勢角度を取得する、ということで検討を進める。
加速度センサーの値が信用できないのはロボットが加速運動している時。逆に言うと定速運動している時の加速度センサーの値は重力加速度のみのはずだから使えるはず。
加速度センサーの値が、重力加速度だけであることをデータから判別することはできないので、ジャイロセンサーからのデータで判別しなければならない。 ジャイロセンサーのデータは角速度なので値に変化が無ければ定速ということになる。ただし、ジャイロデータもふらつくのでそのあたりを考慮した処理が必要。
しかし、定速の判定が以外と難しい。静止している状態でもサーボがびくついている時（見ても判らないレベル）などはジャイロは反応して信号を出す。それをキャンセルするためにノイズキャンセルを判定式に加えると動作中にジャイロに切り替わったりする。
静止しているときのジャイロのふらつき
他の問題も出てきた。静止状態から動きが発生する場合はよいのだが、歩行などずっと能動的に動いている場合、ジャイロデータの誤差が累積して姿勢データは使い物にならない。せめて歩行などの周期動作についてはジャイロにリセットをかけて姿勢角度の信頼性を高めたい。そうしないと、歩行中にバランスをくずして転倒する場合には転倒回避できないことになってしまう。
左右（Y軸回り）は周期動作なのでなんらかの手段が考えられるのだが、前後（X軸回り）は指標になるものがなにもないのでどうしようもないかもしれない。将来的には視覚情報から姿勢を補正するようになるんだろうなと思う。
自律を考えるとロボット自身が自分の状態を客観的に捉えることは非常に大切。 さて、もう少しあがいてみよう。
■９月１７日■
どうにか実用に耐えるであろう、程度の姿勢角度取得が出来るようになった。ジャイロから加速度センサーへの移行条件が厳しいのでちょいと酔っ払い気味なロボットになってしまっているが。
画像は立たせたロボットを前後左右に揺らしたところ。最後は揺れが収まって直立状態で角度が０になっているところ。calc Xというのが前後の角度でcalc Yというのが左右の角度
次は歩いてる時の補正を組み込もうかと思っている。
姿勢角度が出来たので転倒回避歩行に再チャレンジ。
角度監視のクラスを作ったり、それに合わせて色々コーディングを直したり。更には転倒判定の関数を総見直し。考え方を整理してちょっとコーディングを修正したり。
下の画像がトライしているところだが、転倒しないのに「転倒」判定が多すぎる。まだ詳細を分析していないのだが、揺り返し時の速度計算結果の値が大きすぎるような。。バグっぽいなー。
姿勢角度を取得して転倒判定をするようにしてからは、復帰方向の移動に関しては復帰してから反対側に倒れるケースも考えてるし、復帰できないでこけるケースも考えている。 ちょっと手を広げすぎて失敗している感があるな。
復帰方向の動きは「問題なし」として、復帰方向の動きだけ考えた方がよさそうだ。
この三連休は思ったよりはかどらなかったな。 それなりに成果はあったからヨシとしようか。
■９月１８日■
引き続き転倒回避歩行
判定処理のチェックを行ったが、今日見た範囲では大きな間違いは見つからなかった（小さな間違いはいくつか。。） テストしてる間は手でロボットを揺らしたりこついたりしているので終始動いている状態。 すると、姿勢角度はジャイロのオフセットを累積していってとんでもない値になっていく。 すると、転倒判定は「転倒」がつきっぱなし。
ジャイロは前のめりの値を出し気味なのが原因かどうかはわからないが、前方への転倒判定が甘い（倒れないのに「転倒」と判定してしまう。ジャイロのデータも少しノイジーなので平均値を取ってノイズキャンセルして使うといった気遣いが必要なのではないかと思う。
まとめると、姿勢角度のジャイロ→加速度センサー切り替えはもう一度見直す。 転倒判定に使う角速度入力値はノイズキャンセルを行ってから使う。
判定処理の簡素化は、やれば簡単な話なので、もう少し今のカタチであがいてからにしようかと思う。 簡単なんだからささっとコーディングして比べてみるって方がいいのかもしれないな。
それはさておき、ロボットを歩かせたり動かしたりしていると、ガタ付きが気になって仕方が無い。寿命と交換でガチガチに作ることもできなくはないが、それで動作の精度が上がってもなんだかなと言う感じだし、ガタガタですぐコケルのはもっと問題。関節ごとに伸筋と縮筋を持たせることができればガチガチからゆるゆるまでインピーダンス制御できるのだがそれはまだ難しそう。 よって、このガタ付きと付き合いながら制御を検討していかなければならないわけだが、ちゃんと制御しようとすればするほどガタ付きは計算するにはやっかいな要素。遅れ要素になるし減衰も起こるし反射も発生する。その辺りを難しい数式を使わずに処理できればなぁ。そのあたりちょっと検討してみようかなと考えている。
■９月１９日■
昨日思いついたジャイロ→加速度センサー切り替えルールをコーディングしてみる。
今度こそ使えそう。結局、「定速」では無く「停止」を判定することにした。ジャイロは回転運動を見ていて、加速度センサーは並進運動を見ているわけだからジャイロで検出できる定速運動は定速回転。定速回転運動をしている時に並進運動が定速というのは非常に回転半径が大きい時だからジャイロで定速並進運動を検出することは出来ませんでした。ジャイロのノイズ（ノイズじゃないけど）を許しつつ停止を判定するルールを作ってみたのだが、いい感じに見える。切り替えもはやいし、ジャイロが暴れているときも切り替えが適度に発生する。ジャイロが暴れるっていうのはサーボがチクチク動くときで、少し不安定な姿勢のとき。一見停止しているように見えるのだが、微妙にぴくぴくしているのだろう。
これでまた転倒判定をテストしてみたところ、昨日よりはよい感触。 ただし、復帰方向の動きに関しては必ずオーバーシュートして反対側に転倒するとの判定が出る。ロボットを動かしてみた感じと計算結果があまりに異なるので１ケースを詳細に計算してみることにした。
判定ルーチンでは線形倒立振子の式を使っているが、単純倒立振子でも計算してみた。
サンプリングした状態は図のような状態。
初速度12mm/sで復帰方向へ動いている。これで正姿勢になる時の重心点の速度が384.6mm/s そんなに加速するのかーー？って思うが、高さ194mmの位置に3キロのカタマリがあって・・・と想像すると確かにゴトーンって動きそうな気もしてくる。 ちなみに質量は関係ないが、質点を支える構造物の質量イメージもあるので3キロというロボットの重さを当てはめてみた。
下の図は正姿勢に戻っていく様子。単純倒立振子でも線形倒立振子でも大きな差はない。
その速度で反対側に揺り返しがあるのだが、反対側の支点までは43㎜ 計算すると見事に転倒します。
ちなみに初速度を0にしても転倒しました。 あと、試しに反対側の支点までを57mmにすれば転倒しませんでした。
つまりこれは関節がふらふらしたやわらかいロボットと、変形もロスも考慮していないガチガチの計算モデルの大きな差ということ。実際のロボットはこの計算のようには動かずに関節のダンパーでエネルギーを吸収して転倒しないということらしい。
とりあえず、復帰方向のオーバーシュートは判定するのをやめておくことにしよう。 また、復帰方向の動きで復帰できないで転倒というケースも判定しているのだが、これも除外。転倒するとわかったとしても、動いている方向と逆の方向への転倒回避行動をしなければならないことになり、うまくいかないであろう。
結局は復帰方向の動きの場合は計算をせずに「転倒しない」という判定を返すようにする。
あと、剛性が問題なのであれば、ジャイロデータのノイズキャンセル（ノイズじゃないけど）は見送ろうかと思う。
■９月２２日■
転倒判定はほぼオッケィな感じなので転倒回避歩行の処理を見直し。
転倒線と胴体姿勢を受けて、ロボットの姿勢を計算するのだが、一度関節角度にまで分解してから姿勢に再構築。角度の管理はshortの変数で0.1度単位で行っているので角度データから復元すると少しずれが生じる。はじめ、それに気付かず、何でずれるのかさっぱりわからなかった。角度の管理をfloatでやった方が気持ちはいいんだけどな。どうしようかな。
つま先立ち状態からの歩行なので今までのルールでは思い通りの一歩が踏み出せない。歩幅とかスタンスの算出方法に考慮が必要。
前につんのめった状態から一歩を踏み出す。支持脚となる足で、重心点の高さを保ちながら遊脚を前に出すのだが、初期状態から既に支持脚が後ろにある。前に伸ばせる長さがほとんど無くなる。さらに、重心点が支点から随分と前にあるわけだから高さを保つための加速度は相当必要となる。つまり、大して大きな遷移時間は取れないということだ。いままで0.3秒までで考えていたが、0.2秒くらいまで考えないとだめかもしれない。サーボついてこれるかな。。。
動作可能な範囲の一歩では十分に安定した姿勢にまでは移行できそうにない。初期姿勢によっては数歩で対応しなければならないようだ。数歩での対応は一度トライしてあきらめた経緯があるのだが、状況が変わったし、結果も変わるかもしれない。再トライだ。
姿勢角度の取得について、「減算型フィルター」というのがあることを教えてもらった。
加速度センサーをジャイロセンサーで補償するカタチなのだが、ジャイロはHPFを通して、加速度センサーはLPFを通してから合成するらしい。フィルターの特性にキモがあるようだ。
加速度センサーにLPFは既に通していて、通さないと値が暴れて使えない。 ジャイロの方だが、デジタルフィルターでHPFを構成するのってどうするんだっけ？と思って調べたらやはりFIRフィルターでゲインをマイナスにするってことらしい。ほほうと思ったけど、これって微分だな。微分フィルターには既に通していて、ジャイロセンサーのデータを微分するとはちゃめちゃな信号ができてしまって使えないっていうのがこないだの結論だったのだ。ううむ、すると、、、HPFじゃなくてBPFがいるのだろう。高い周波数成分は飛ばしちゃって低いところも飛ばしちゃっていいところだけとって使う感じ。いままでなんちゃってFIRフィルターで平均値を取るだけだったのだが、ちゃんと周波数特性を考えてゲインを計算しなきゃだめなようだな。
あと、離散データといっても25ms周期のデータだと粗すぎる。微分したら使えないのはこの辺が原因だろう。せめて1ms、出来ればそれ以上細かい離散データなら周波数特性、特に高周波域の特性をきちんと設定する価値があるかもしれないな。 あと、ジャイロデータにはサーボのピクピクが乗っかっている。ノイズが乗っているのではなく物理的に振動しているようなので仕方がない。サーボのピクピクも周波数を特定して選定除外すべきだ。
今使っているセンサーボードはロボットキットについてきたものそのままなので改造は無理。やるなら自作で作らないといかん。ちょっと今はできないなぁ。
歩行中の姿勢取得についてはまだ解決していないので、FIRフィルターでのBPFも試してみよう。
以前、音声認識をやってみようと思ってデジタルフィルターについて調べたことがあった。FIRフィルターというのはそのとき見知っていたのだがそのときはFFTを知りたかったので流してしまっていた。で、ある周波数成分を選択除去したかったのだが、FFT（正確にはDFT)を使ってスペクトルを作り、処理してからIDFT（逆フーリエ変換）したりしてた。FIRでやった方が処理は簡単だったんだろうなー。orz ちなみに音声認識は母音の認識しかできませんでした。(^_^;)
■９月２３日■
引き続き転倒回避歩行の歩容処理ルールの検討。
今回は入力速度の減速というのをしないでやってみようと思ってます。複数歩数対応がキモ。 そのためにいままでは管理しなかった支持脚の動作域（なぜ！？→めんどうだったから）を管理して、重心点の動作速度に限界速度を設けて、遷移時間にも制限を設けて、対応できる幅での一歩を踏み出すという感じでやろうとしてます。
遷移時間は最大0.4秒。これ以上長くすると安定して歩けません。短い方は0.05秒でも動くことは動く。ただし、どの程度リニアなのかはわかりませんが。 問題は動作限界速度。普通に歩かせてついてこれる速度はまぁ50mm/0.3秒くらい。 だが、末端速度はもっと速くて400mm/s（倒立振子動作だと末端速度が最も速くなる） 。これは重心点の動きで、遊脚の動きは2倍の100㎜となります。さらに、遊脚は両足支持期間（加速期間と呼んでいる）が4割ほどあるので、100mm/0.2秒、つまり500mm/sで動いていることになる。ちなみに遊脚はリニアに動かしているので平均速度で良い。ただし、遊脚は重心点の先についているのだから、重心点の動きにのっかってその速度で動いているから。一番負荷が大きいのはどのサーボなのかなー。。。 ということで、重心点の末端速度が400mm/s以下となるように計算することにした。
入力速度や初期姿勢によっては計算が成り立たなかったり、遷移時間がゼロ（インターバルサイクルの0.025秒以下ということ）になったりするので、これはもう回復不能として処理することにしよう。
一通りコーディングできたが、デバッグは明日。 今からやるときっと寝られなくなる。
■９月２４日■
朝から一日デバッグ。 不調なので一日と言ってもそれほどつきっきりというわけじゃない。
それでもがんばって何とかいけそうだというところまで持っていった。だが、実際に動かしてみると妙な姿勢にばかりなる。ログを調べると、転倒判定の時と転倒回避歩行の計画の時で軌道エネルギーの値が変わってきてしまっているのだ。
処理の流れはこんな感じ
＜転倒判定＞
転倒判定では支点になる軸を調べる→重心点との位置関係を調べる→軌道エネルギーを算出し、＋なら転倒、－なら転倒しない。
＜歩行計画＞
転倒判定時の情報を受け取って、胴体姿勢と転倒線から初期姿勢を構築→支持脚を決め、支持脚と重心点との関係を調べる→支持脚の最大移動距離を調べる→
その時の重心点移動速度と遷移時間を調べる→マックス値と比べて調整→遊脚の着地点を決める。
大雑把にこんな感じ。ここで、遷移時間を調べるときに軌道エネルギーを使う。ところが、転倒判定時と歩行計画時で軌道エネルギーが一致しないので判定が来るって遷移時間の計算が出来なくなるケースが多発するらしい。 ・・・その他の問題もありそうだが。
なぜ、軌道エネルギーが一致しないか。 それは、転倒判定時の重心点の位置の算出が手抜きだからだ。 ま、見直すと、歩行計画時の初期姿勢構築にも間違いがあったのだが。。
本来は転倒判定には三次元倒立振子モデルで計算すべきなのだが、一点で接地している状態をコントロールできるはずはないので2点での接地を前提に考えている。この2点を通る直線を転倒線と呼んでいる。
ロボットは転倒線を軸に転ぼうとしている、と断定して計算しているのだが、この計算を回転行列で表すと、
rR * rP * rT = rf * ra * r(-f) * rr * rp * rt
rtとかrpというのは回転行列を表していて、ｔはX軸、pはY軸、rはZ軸の回転、fは転倒線の角度なのでZ軸、aは転倒線を軸とした回転を表している。rtpは元もとのロボットの胴体姿勢角度、RPTはグローバル空間上のロボット姿勢を表す。このうちTとPはセンサーで取得した数値が入るが、Rというのは計算上現れるだけでセンサーでの取得はとりあえず出来ない。
ここで知りたいのは a の角度である。が、Rも判らないから解けるのかな？
で、いいや、ってことで、正確さは欠くが簡単にR-r P-pって感じで差分を取ってそれを転倒線に適用して転倒角度を決めていた。
対して歩行計画では胴体姿勢角度と関節角度から姿勢を構築しているからいわばリアル。多分こっちは間違えないはず。 転倒判定は毎回行うけど、歩行計画は転倒判定で陽になったときだけだから・・、と思っていたのだが、さっきの行列式を解くのなら姿勢を関節角度から構築するのとどっこいどっこいだなー。
この件はもう少し時間を置いて検討しよう。 あせると時間を食うばかりで失敗しそう。
■９月２５日■
時間をおいて、、とか言っていたが、気になると他の事を考えられなくなる。仕方が無いので一生懸命考えた。
まず、胴体姿勢から転倒しかかっている姿勢を構築する件
転倒線を軸に傾いた状態を姿勢として構築する。転倒線をX軸とする方向を正面とする姿勢とする。
足の関節角度は動いていないので、元の姿勢から
Rot(x,a)・Rot(z,-f)・[lbp]
となる。ただし ｆは転倒線の角度、aは転倒線を軸にした傾き角度を表す。
だが、a が未知なのでこれだけでは計算できない。判っているのは回転後の胴体角度 T, P なのでここから算出する。
①構築する下半身姿勢のtprを
t=T, p=P, r=0 g=(0,0,0)
として下半身を構築する。足姿勢の構築時も
t=T, p=P
とする。
重心のずれがoffsetとして現れるのでg（重心オフセット）に移し変える。
②転倒線は足部の接地点となる２点を結んだ線なので、求める姿勢は必ず両足が揃った姿勢となる。
だが、①では r を考慮していないので必ずしも depth = 0 とはならない。そこで、
r = - atan(widh / depth)
として
width=sqrt(width^2+depth^2)
depth=0
roll_[RIGHT] += r
roll_[LEFT] -= r
とする。 r の設定に応じてgを回転させる。
g.Rotz(r)
以上 (9/30誤記訂正）
注意１
①で、
t=T, p=P, r= r - f
とすればいいように考えてしまうが、T,P は
Rot(x,a)・Rot(z,-f)・Rot(z,r)・Rot(y,p)・Rot(x,t)
となった結果である。p<>0の場合は a,-fの回転で z軸回転角度が影響を受けてしまうため、正しくない。
注意２
前提として転倒線を軸にしての回転としているが、実際には必ずしもそうではない。その場合、姿勢構築にてhung<>0となる。
次に転倒判定時の姿勢換算
胴体姿勢は
Rot(z,r)・Rot(y,p)・Rot(x,t) =
上半身同期動作
ラムダの４足歩行[crcp] [-srct+crspst] [srst+crspct]
[srcp] [crct+srspst] [-crst+srspct]
[-sp] [cpst] [cpct]転倒線に対する傾きで表すと
Rot(z,f)・Rot(x,a)・Rot(z,e) =
[cfce-sfcase] [-cfse-sfcace] [sfsa]
[sfce+cfcase] [-sfse+cfcace] [-cfsa]
[sase] [sace] [ca]
これを整理して
-tan(f) = sfsa / -cfsa = srst+crspct / -crst+srspct
sin(a) = sfsa / sf = srst+crspct / sf
sin(e) = sase / sa = -sp / sa
換算式まではできた。f の算出にatan( )を使っているので、答えによってはちょっと調整が必要。
これで胴体姿勢から、「ある転倒線を軸に回転している」という表現に換算できる。ただし、胴体姿勢から一意に転倒線が決まってしまうので胴体姿勢の r と、転倒線角度の f でやり取りが必要になる。
まぁ、これで転倒判定のたびに下半身姿勢を構築しないで済むはず。実際に転倒判定に適用するにはもうちょっと整理が必要。
■９月２７日■
胴体姿勢の件はさっぱり検討違いでした。
胴体姿勢を得るには単純に法線ベクトルを求めればよかっただけ。胴体姿勢角度から法線ベクトルを求めて、ニュートラルの姿勢角度分回転させたベクトルが転倒方向と転倒角度を示すベクトルになる（はず）。
25日に書いた行列計算はえらく面倒な上に、アレでは転倒方向と転倒角度は算出できそうにないです。
つまり、支持多角形から転倒線を決定していたのだが、胴体姿勢から転倒線を算出することになり、転倒線の算出は無意味ってことになります。数値的には片足が浮いた状態からの歩行となる。片足が浮いた状態から一歩を出すのは無理だな～。
＃関東組練習会が近所の武蔵溝ノ口で開催されることになったようです。せっかくなので皆さんのロボットを見せてもらいに参加しようと思います。ラムダはラジコン操作できないので「練習」できませんが、よろしくお願いします。
■９月２９日■
先日渋谷の本屋へ行った時、いい本をみつけました。
ロボット工学 森北出版株式会社 1999年初版
150ページくらいの比較的薄い本で、1800円
内容はアマゾンによると、、関節形アームを中心としたロボット工学の一般的かつ入門的な取り扱い方法、1自由度多節リンク機構を対象にした機構学・機械運動学の基本的な概念、力制御、時間とエネルギーの最適制御の方法について詳述。いずれも高度な予備知識を必要としないよう、力学、ベクトル論、制御の基本に立ち戻って解説。
てな感じで、動力学方程式の立て方とかが簡単な、でも簡単すぎない程度のロボットモデルでテキパキといい感じで説明しています。もちろん薄いし、レベルは高くないので、載っていることは基本的な事ばかりなんですが、判りやすいというか、、この本の次に読む本はもうちょっと難しい本がきちんと読めそうな気になるような本です。ちなみに動力学はマスターしたって人には用無しな本です。
アマゾン見るとユーズドがいっぱい出てる。半額くらいだ。。マスターしたら売っちまうのか、大学で教科書として使われているのか？
転倒回避の方は、法線ベクトルを使った転倒方向と転倒角度の計算が出来て、転倒しかかっている場合は支持多角形からの転倒線ではなく、法線ベクトルの傾きで転倒角度を算出するようにしました。 支持多角形から算出した場合とほぼ同じにはなるんだけど、関節のガタ付きなんかで若干は違う値がでます。
それに合わせたコーディング修正をして、転倒判定はほぼ固まった。
転倒する速度であれば、胴体姿勢が垂直の時から転倒を予測してます。実際に倒れていないのは手で支えたから。
転倒速度がゼロであっても傾きがある程度異常だと転倒判定します。
ところがまた問題が。
たまにこんなグラフが。ドカンと立ち上がっているのは左右方向の姿勢角度。なんかの拍子にGセンサーに衝撃が検出されるとこうなってしまうのだ。ドカンと立ち上がってる同じタイミングでちょこんと立ち上がっているのがその衝撃らしい。 立ち上がった後にジャイロに切り変わるのでしばらくは間違えた角度のまま。ジャイロからGセンサーに切り替わる時に正しい値に変わる。 大抵は転倒するほどの外力に伴って検出されるので転倒方向の判定に間違いがでてしまう。どういう力がどんなふうにかかるのか？？？ いつも出るわけじゃなくタマにでるからタチが悪い。
ちなみにこの時は左右の揺れは与えていません。
とりあえず、監視しつつ、次に進みましょう。もー、、全然すすまねぇなぁ。(ーー;)
姿勢角度取得はジャイロとGセンサーを切り替えて使っているのだが、まだ色々と問題がありそう。
減算型フィルターを採用するにもデータの取得周期が25msではデータが粗過ぎてうまくいかなさそう。ということで、センサーデータの取得周期を上げる方向で検討してます。
ジャイロセンサー
Gセンサーと一緒になったのもあるのだが、ちょうど品切れ。 Gセンサーだけなら安いのがあった覚えがあるのでジャイロだけのこれを買ってみる。２軸で9450円とお得です。感度も500deg/s と、広い。 sparkfunって有名なのかな。教えてもらうまで知りませんでした。
加速度センサー
これは3軸で800円。 ±２G。 ベステクにもっとレンジの広いのがあったのだけど、加速度は絶対値で取得できるからレンジオーバーになることがあっても構わないかな。ということで、安いという点でこれを採用。秋月電子通商。
これをatmega32のCPUボードにつなごうかと思ってるんだけど、どれくらいの周期でデータ取得できるだろう？変換速度を200μｓとして、5チャンネルあるから、データ取得だけなら2ms周期くらいで可能か。これと通信を同時に行うことを考えるともうちょっと粗くなるかも。1ms周期は無理っぽいな。
■９月３０日セカンドステージ■
転倒回避は、転倒判定は一応固まったということで、何かあるまでいじらない。回避歩行計画は、転倒初期姿勢の計算方法にいくつか間違いがあったが、いろんなケースで検証して一応大丈夫なはず。
初期姿勢が固まると、おかしな歩行計画を出すこともなくなった。耐久テスト的な動作はさせてないのでまだわからないが。
色々試した結果をメモ的に書いておく。
・遷移時間は0.2秒くらいならなんとか転倒に対応できる感じ。
・一歩終わった時点で重心が前にあるとまず立っていられない。
・複数歩での対応はやはり難しそう。
・転倒回避歩行の開始はこないだ判定を諦めた、「反対側に傾いている時」から開始しないと成功率は上がらない。
・高さを保存したままでの回避歩行は難しい。標準高さ（腰の高さ)210㎜だが、転倒回避は180㎜くらいにまでは高さを落とすべき。
・重心位置での転倒判定はボーダーを越える前に「転倒」判定を出すべき。ボーダーを越えてからだとまず間に合わない。
・胴体姿勢から開始姿勢を算出するとどうしても片足が上がった状態からの歩行開始になる。（実際には両足着いているが） 片足を上げた状態からの歩行をサポートすべき。
・関節のインピーダンスの低さを逆に利用して、外力を逃がすといった検討も必要かもしれない。
今のラムダのサーボと体重ではそろそろ限界を感じてきた。速度とトルクとウェイトに対応したロボットの設計を待つべきかもしれない。ラムダ２号については色々と考えてはいるのだが、リンクを使ったインピーダンス制御をやりたいなと思っている。電流制御も必須だし、悩ましい。
転倒回避歩行も終盤に近づいてきたので、次の課題の歩行のフィードバック制御について検討を開始する。
歩かせているとだんだんと同期が外れてきて、一歩飛ばしてしまったりすっころんでしまったりする。振子なんだから支点から重心点までの腕の長さには固有な周波数があって、それから外れると不安定になるのかなと思ったりしたのだが、その辺りは初速で調整していることになる。やはり問題は質点ではなく剛体でもなく二重振子でもない点なのかとも思う。とにかく傾向を把握するためにちょっと実験をしてみることに。
歩かせるとすぐこけてしまって実験にならないのでその場で足踏み運動をさせる。歩幅とスタンスを考慮してスタンスを180㎜にして動作させてみた。
スタンス180㎜ 足上げなし
左右の振幅のみ。緑がジャイロデータ。ちょっとだけ位相がずれているが、ほぼ同期している。
スタンス180㎜ 足上げ20ミリ
足上げの反動でジャイロデータはこんなに乱れる。
スタンス180㎜ 足上げ10ミリ
足上げ10ミリならジャイロもあまり暴れない。
スタンス180㎜ 遷移時間0.3秒 足上げ10ミリ
さっきまでは遷移時間0.4秒 0.3秒にするとジャイロが暴れだす。
スタンス180㎜ 遷移時間0.3秒 足上げ20ミリ
暴れているが足上げ動作が同期しているようで振幅が大きくなっている。こけそう。
足上げ動作による波形の乱れを抑える必要がありそう。 だが、その場での足踏みでは歩行中のような顕著な同期外れは起こらない。前後動作を加えることで同期外れが起こる原因が発生するようだ。もうちょっとテストのパターンを増やしてみよう。