↑ 教科書通り（それでも判る範囲の誤記は修正したけど）で生成させた重心軌道
開発日誌(43)
トップへ戻る
開発日誌インデックスへ
前のページへ
次のページへ
■２０１６年６月３０日■
久しぶりの更新になります。作業して、日誌更新してってのはホントに大変なんだけど記録の意味も込めて更新再開したいと思います。
さっそくですが、今後の活動方針を。
まず、いままではZMP規範の歩容生成で、そこそこ歩けたのだけどロバスト性が低い。それを打開する方針として軸分離型の関節構造を考案しました。更には足裏センサーでZMPを取れるようにしたのでそれを歩行安定化に利用したいと。
今までの歩容生成はオフライン生成でしたので、厳密なデータから厳密な計算を行い、安定して歩ける歩容を得る事はできました。ただし、サーボの運動性などを反映することはできていなく、更に致命的なのは外乱に弱いということでした。
教科書の最後のお題としてオンライン生成法が載っています。私の持ってる教科書（ヒューマノイドロボット初版）では誤植（じゃないか、誤りですよね）があって、まともな計算結果は出ませんでした。が正しい計算式を梶田先生から教えてもらったので見事に歩けそうな計算結果を得る事ができました。 ※最新版では訂正されているそうです。
↑ 教科書通り（それでも判る範囲の誤記は修正したけど）で生成させた重心軌道
↑ これが正しい数式で生成させた重心軌道 すばらしい！
あとはこれに各種センサーのフィードバックを入れることができれば更に高度な歩行ができそうです。
ここまでは既定路線。計算式が解けたので、やればできちゃいそうだし、結果も大体想像つくなぁと思うとモチベーションが上がらないなと。ですのでやってもうまく行くかどうかわからんことをやりたいと思います。
ということでDEEP LEARNINGを取り込みたいと思います。
もともと自律型ロボットが作りたくてロボット作ってるので、20年以上前の人工知能ブームからニューラルネットワークまでひとしきり勉強したり実験プログラム作ったりしてました。なので、DEEP LEARNINGが脚光を浴びるようになってそれなりに記事を読んだりしていたのですが、クラウドを使ったアプリケーションだと思い込んでいてあまり触手が動きませんでした。もう少し詳しく知る機会があって、開発環境及びその進化の状態に驚愕しました。
今は自動車の自動運転や、ドローンの制御（なのかな？）もDLモジュールでやる時代なのですね。
と言う事でラムダにDLモジュール積んで歩かせたいと思います！
カメラ画像をDLモジュールで解析して状況分析させるのもアリだけどこれは既定路線かなと。歩容生成は難しそうだけど安定化制御ならなんとなくできそうな気がしないでもないな、ということで当面のテーマはこれで行く事にしました。DEEP LEARNINGが今後どのような地位に落ち着くかは不明だけど、NVIDIAが頑張っているうちはいろんな新しいことが起こりそうなので正しくついて行きたいと思います。
思い立ったが吉日ということで組込み用DeepLearningModule(NVIDIA JETSON TX1)を購入しました。結構高い買い物です。(^_^;)
↑ 放熱フィン部分がモジュール本体。 名刺サイズくらいです。これだけで消費電力10W 処理能力で考えると驚くほど低い！！！
すぐにDLモジュールが活躍するとも思えないんですが、まぁいいでしょう。こういうのは勢いです。
歩行を学習するには歩行データを山の様に用意する必要があり、その教師データが必要です。実際のロボットを歩かせると時間的にもハード的にも無理があるのでシミュレータを使えないかと思ってます。
ロボットの物理シミュレータとしてはテクノロードの「GO Simulation!」があるのですが、以前導入を検討した際にはリンク構造には対応していないとのことでしたので見送りました。
改めて調べるといつのまにか対応されているようです。でも、作例だと並行リンクなんですね。ラムダに並行リンクの場所なんてほとんどありません。（足首ピッチ軸は股関節部分から並行リンクで動かしてます）杉浦さんに聞いてみると、かのフロスティーのモデリングを途中までやったと（途中まで？）フロスティーが出来るならラムダもできるだろう、もしダメでも杉浦さんがなんとかしてくれるだろう、と言う事でGo Simulation!上でほぼ実物並みのリンクでモデリングすることにします。
↑ 左からInventorの設計データ、Goシミュ用、Goシミュに取り込んだところ（まだ途中です）
Goシミュ上で歩かせるにはモーションファイルを作るか、歩容生成関数をDLLにして作るかしないといかん。 早いとこ歩かせよう。
いやいや、しまけんさんそれ違うよ！とか、ん？それってどういうことですか？って人はこちらまで。
■２０１６年7月３日■
Go Simulation!上でのモデリング（腕以外）が完成しました。
動きに影響する部分のリンクはすべて再現しました。
←浮かせて
←落とすと、膝をかがめてしゃがむ
←そして立ち上がる
サーボのトルク設定やバネ設定がむちゃくちゃですが、リンク構造の動きは再現されていますね。
本物は膝にアシスト用にバネを入れているのですが、それは設定されていません。 杉浦さんに確認したところ、サーボはバネモデルを使って計算しているそうなので、サーボゲインをバネっぽく設定することでできそうです。
モデル構築のために３ＤＣＡＤデータで位置寸法を取得しながら設定していったのですが、GoSimulationのモデルポリシーというのかな、を理解するまではキビシかったです。
ＣＡＤ設計時にはオブジェクトの中心なんて考え方は無いので、計算したり、ＣＡＤに寸法採取用のオブジェクトを追加したり。。
あと、位置設定のための数値指定が小数点以下１桁までなのですが、せめて２桁欲しいなと。せっかくキレのいい数値で設計しても、斜めに取り付けた場合は小数点以下１桁じゃ収まらない場合が普通です。
次は、トルクの設定をして、ＤＬＬを作ります。 細かなリンクは省略したので逆キネ関数も少しいじる必要が出てきました。
ご意見、ご質問はこちらまで
■２０１６年7月４日■
サーボ設定とバネ設定の検討。
解析上のサーボはバネとほぼ等価だそうです。 実際にはサーボにはデッドバンドとかパンチとかが有って、リアルなバネとは違うのだけど、解析上は一緒だそうです。
機械要素のモデル化ではバネ・ダンパモデルというのが使われて、弾性係数と粘性係数で表されます。
・弾性係数を大きく、粘性係数を小さくするとバネ
・弾性係数を小さく、粘性係数を大きくするとダンパー
になります。弾性やら粘性を現象で表現してみると、
弾性 ⇒ 変位に反応する→変位に応じた応力を生む
粘性 ⇒ その場に留まろうとする 変化を許さない 変化しようとする力を吸収する
って感じですかね。
ＯＤＥドキュメントによると、ジョイント部分に適用するパラメータにＥＲＰとＣＦＭと言うのがあって、ERPがジョイントの誤差を修正するパラメータ、CFMが拘束力混合パラメータだようです。
実際の解析におけるバネやサーボはこのＥＲＰとかＣＦＭの設定で行うそうです。
ＯＤＥのマニュアルによると
ERP = dt*kp / (dt*kp + kd)
CFM = 1 / (dt*kp + kd) ただし、dt：サンプリング時間 kp：弾性係数[N/m] kd：粘性係数[Ns/m]
だそうです。
ODEは速度ベースで処理しているらしく、ERP、CFMはそれぞれ速度ベースに整えられているということらしいです。
で、Go Simulation!のサーボ設定ですが、Pゲインが弾性係数 Dゲインが粘性係数に相当するそうです。 デフォルトでのサーボ設定はPゲイン：3276.5 Dゲイン：88
これを先ほどの閑散式でERPとCFMに変換してみると ERP=0.157 CFM=0.0096
ODEデフォルトのパラメータがERP=0.2 CFM=0.0001 らしいので、CFMが少し大きく設定されているようです。
今のサーボは静止状態なら荷重を保持しているくせに10ミリの高さから落としただけでクシャーーーっとへしゃげてしまうくらい柔らかいので、もっと固くしたいですね。歩いたらポンポン跳ね返っちゃうくらいに固くしましょう。
固くするにはERPは大きく、CFMは小さくする必要があります。 両立するkp、kdを考えると、、、、、、kpもkdもでかく、かつ、kp>>kd にすればいいのかな。
ちょっとサーボ評価のモデルでも作って評価してみるかな。。。
というか、サーボのモデルって誰か作ってないんですかね。(^_^;)
ご意見、ご質問、サーボのモデルが情報はこちらまで
■２０１６年7月１０日■
サーボとバネのモデル検討をしているところに注文していた母艦PCが届きました。
ディープラーニングでは学習時にべらぼうに計算をしなきゃならないということで、TX1の母艦用にPCを新調しました。
ディープラーニングが進歩した背景にGPGPUというのがあって、グラフィックプロセッサーを使ってグラフィック以外の分散演算もやらせるということが実用化してきた背景があります。
更には前処理のCPUやデータを与えるためのメモリーなども学習の速度に影響するということで、もりもりスペックの最新ゲームマシンがディープラーニングの母艦に適しているということです。
実際、法人向けディープラーニング用マシンというのが売ってて、それが今回買ったゲームPCとスペックがほとんど一緒だったという。。 （研究向けは100万円からって感じでスペックももっとすごいです。）
↑来たのがこのPCゲームマシンらしくかっちょよさげなカバーがついてます。 下のウォーズマンの口の辺りが青く光ります。逆にかっこ悪い上に光学ディスクの出し入れに邪魔なのですぐ外しちゃいます。
普通のPCっぽくなりました。天井部の手前にUSBとかメモリーカードコネクタとかがついていて便利です。
こいつはディープラーニング専用機にしようかとも思いましたが、高価なGPU積んでいるのにODEとか３DCADに使わないのはもったいないと思ってWindows10プレインストールにしました。 Linuxとデュアルブートにします。
↑GoSimulation!もInventorも色々苦労しながらもちゃんと動きました。さくさく動く！バージョンのせいか、グラボのせいか、発色が良いです。作業用画面なのにイイ感じ。Goシミュもちょっと早くなりました。 Inventorはこの機会に2017にしました。（現時点でWindows10での動作が確認されているのは2017だけみたいです。アップデートで上げたのは動く可能性大だけど、クリーンインストールだと少し敷居が上がるとか。
今回、WindowsもLinuxもSSDに入れました。更にデータ用に２TBのHDDも積んでる。２TBなのに6000円切ってるんですね。安くなったな。メモリーはちょっと見劣りして16GB これはいつでも増やせるからあとでいいかと。後になるほど安くなるし。
↑LinuxはUbuntu。 これも色々苦労しました。nVIDIAボードが搭載されているとインストールでひと悶着あります。CUDAもcuDNNもボード対応版じゃないとダメとか。 できるだけ動作実績があるものを使うようにしてるんだけど、新しいの買っちゃったのでこれは仕方ないですね。
Caffe、Python入れて、Torch、Lua入れて環境は整いました。
↑最近のLinux事情はすごいですね。USBに挿せばカメラもすぐ使える。 これはロボにサバゲの時に調達したオートフォーカス付きWebカメラでTX1の自撮り
で、大体環境は整ったかなというところなんだけど、この土日はLuaというスプリクトからespeakというボイスシンセサイザーを呼び出すというモジュールのインストールではまってしまいました。解決してません。解決の目処がありません。
これができると、NeuralTalk2というディープラーニングを使った判断器で画像にアノテーションをつけるというまさに人工知能的なデモで、アノテーションをしゃべらせる事が出来るというものです。（インターフェース誌7月号の掲載記事） 別に必死にやらなくてもいいんだけど乗りかかった船感と、生成した文をしゃべるのを聞きたかったのでがんばったけど今のところダメっぽいです。誰か解決してください。
環境構築してて、いまどきはディープラーニングのワークフレームのフロントエンドにPythonやらLuaやらのスクリプト言語を使って連携しているケースが多くて、その機動力の高さが売りなんですね。このスタイルに慣れていかないと全然追いつけないですね。
スクリプト言語といえばPeｒｌと思ってたけどrubyになってPythonになって、、、ついていくの大変ですね。
ええと、次は何をすればいいんだっけ？？
ご意見、ご質問はこちらまで
■２０１６年7月２３日■
パズルやクイズは得意なわけじゃないし、どちらかと言うと「嫌い」なのだが、人から見ると「パズルやクイズが好き」に見えるらしい。
どうも、人から見ると取りつかれた様に熱中しているように見えるらしいのが、実際は解けないからムカつきながらも降参したくなくて諦められないだけなのだ。自分では「嫌い」と思うのも、取りつかれてしまい、時間を拘束されるからなんだと思う。
というわけで、torch-espeakのインストールは諦めきれず、エラーメッセージから類推した、「ダイナミックリンクライブラリのシンボルを抜き出して関数を呼び出す」という方法でソースを修正することで動かしました。
ライブラリをスタティックリンクすれば解決するような気がするんだけど、そうしても見たことないエラーがいろいろ出るのでソース修正しないで動かすことはさすがに諦めました。
ただ単にespealkのライブラリーを呼び出してパラメータを設定して動かしているだけのプログラムをコツコツとシンボルで呼び出してポインターを取得して呼び出すという修正をして動かしました。
さて、これでNeuralTalkで生成したアノテーションをシンセサイザーでしゃべられることができる！ と思ったら、luaからOpenCVを呼び出すモジュールが動かない。
これはなんと、torch-opencvのインストーラがmakeするだけでインストールしなかったのが直接の原因らしい。 何度ライブラリのコンパイルやったと思ってんだ"(-""-)"
でも、ネット検索を更に一生懸命やったところ、nVIDIAのサイト内の掲示板にJetson TX1にOpenCV3.1を入れる記事を発見しました。これでコンパイルし直し。これがコンパイルオプションが一番まともそうなのでまぁよかったかなと。
さて、デモは多分もう絶対動くのでラムダに戻ることに。
GoSimulation 状でラムダを歩かせたいと思います。
そのためにはDLL作ってGoSimuから呼び出して動かさねばなりません。これはDLLのサンプルプログラムを参考に。まずは
簡単に屈伸。 二重不平衡リンクによる直交分離のおかげでエレベータサーボ（上下動用サーボ）を動かすだけで屈伸ができます。
じゃ次。
逆キネで動かします。屈伸しながら胴体で円を描く。これはモーションで作るの大変でしょ。逆キネで関数で動かしてるっぽい。円を描けてるようなので逆キネもリンクも大丈夫のようです。
ちなみに、直交分離といっても完全にリニアに分離してるわけじゃないので逆キネできちんと動かした時はエレベータサーボだけじゃなくスライダーサーボ（前後動用サーボ）も少し動きます。
さて、次はいよいよ歩容生成プログラムの組み込みです。今回はオンライン生成。でもセンサーフィードバックなしで作ってみます。
■２０１６年８月２日■
さて、「Go Simulation!」上でIKで動かす事が出来るようになったので次はとうとう歩行です。
いままで歩行はモーションを作った事がなく
その１ 倒立振子モデル
その２ 二重倒立振子モデル
その３ ZMP規範のオフライン歩容生成
とすべて計算で歩容を生成してきました。
一番出来がよかったのはもちろん「その３ ZMP規範のオフライン歩容生成」ですが、準備が大変、計算量が多い（メモリーがたくさん必要でマイコンじゃ動かせない（と思う））、そしてフィードバックを取り込むことが困難。
ということで今回は
その４ ZMP規範のオンライン歩容生成
で行きます。
これだと足裏から取得したZMPをフィードバックすることもできそうです。目標（ZMP）は事前に用意する必要があるけど、モーション自体はその場で生成しているので、モーション自体を事前に生成するオフライン歩容生成よりは柔軟性が有りそうです。計算量もべらぼうに少ないし使用メモリー量も少ないです。
ただし、基本的にロボットのボディー情報を必要としないので逆に言うとフィードバックを入れて初めて完成するともいえます。
まず、目標ZMPの生成。 これはオフライン歩容生成の時に作った関数がそのまま使えます。
次にモーション生成。 これも基本的にはオフライン歩容生成の時に作ったものが流用できそうです。
オフライン版では各部位（質点）の位置、速度、加速度、姿勢を継承する必要があったのですが、オンライン版ではその辺りはすべて不要です。
ですが、1フレーム毎に計算するとは言え、動作の一コマを生成するためには状態を保管しておく必要があるので一時変数を静的変数に切り替えたり、リストで受けていた目標ZMP列をコマで受けるようにしたりと、それなりに手を加える必要がありました。
更には今回はDLLにこの関数を入れて動かすということでデバッグが困難でした。
デバッガとかIDE環境でのプログラム開発ってやったことないのですが、この状態でもIDEのデバッガは使えるのかな？使い方知らないので色々手さぐりでやりましたが、デバッガが使えたらもっと簡単に動いたかも。
で、動いたのがこれ
歩行モーションっぽいのはできてるみたいです。っていうか、シミュレータすごいですね。リアルにダメ歩行させた時の動きをしてますね。(^_^;)
どうもサーボがついてきていないようなのでログを取ってみました。
全然ダメです。サーボがついてきていません。
サーボに角度指示を出してから実際にその角度になるにはもちろん時間遅れが有りますが、これでは遅れが大きすぎてピークに達する前に次の指令が入ってきてピークが出ていないようです。
サーボの特性を見直す必要があります。（デフォルトのままなので見直すってのもなんですが。。）
サーボの速度を見直して、まずは期待通りにピークは出るようにしてみました。
すばらしい！この、下半身だけの機体ならフィードバックなしで歩けそうです。（理想に近いサーボになってますが。。）
サーボの追従具合は
こんな感じです。遅れなしですね。
ちゃんと歩いているように感じるのでこれならZMPをフィードバックできるんじゃないかと、ZMPを取得してみました。
うーん、目標のグラフを並べないと何だかわからないくらい崩れてます。こんなに暴れたデータをフィードバックして発散しないでいられるのでしょうか。わかりませんが。。
（このデータ取得時はサーボ設定も少し緩めて、遅れが少し出るようにしています）
ZMP規範のオンライン歩容生成にはリカッチ方程式の数値解から予見制御ゲインを算出します。この数値解を出すのに重心高さと離散値が必要です。 このリカッチ方程式の数値解を解くプログラムが作れなくて今はoctaveで予め計算し、プログラムに組み込んでいます。重心高さも姿勢により上下しますので、Inventorを使って算出しています。
つまり、歩行時の重心高さを変えたり、サーボ制御周期を変えるとoctaveを動かして予見制御ゲインを計算し、プログラムに取り込む必要があります。
実機の時はどうするか別途考えるとして（Edisonにoctave載せれればいいんだけど）今回はPC上のoctaveをDLLから呼び出して数値解を取り込むことができそうです。
重心位置は、オフライン生成の時は質点モデルを必要としていたのでおのずと重心計算出来たのですが、結局今回も質点モデル作らないとダメなわけですね。
何点かプロットして近似値出してもいいんですが、腕がついたりするとそれも使えないのでちゃんと作りましょうかね。
そういや、絶対動くと思っていたNeuralTalk2+espeakですが、すんなりとは動きませんでした。(>_<)
Make; make test; make install と順調にコンパイル・インストールできた上に、マニュアルで操作した時には確かにしゃべったのに、Luaからスクリプトで動かすとエラーで止まる。(+_+)
試行錯誤した結果、イニシャライズコマンドでエラーを起こしたのちにイニシャライズしなおした時だけちゃんと動くという不思議仕様になってしまいました。
でも、スクリプトでエラーを起こすとLuaも終了してしまうので何とかせねばなりません。
ここから少し長い旅に出るのですが、ターミネートというコマンドでエラーさせても同様にうまく行くケースを見つけることができてわかりました。
ダイナミックリンクライブラリを閉じちゃいけないんですね。開いたままで使えば正常に動くことがわかりました。
マニュアルモードでイニシャラニズコマンドをエラーした場合、ライブラリがオープンになったまま次のコマンドを入力できるようになったため、うまく行っていたようです。わからんもんです。
ということで、NeuralTalk2+espeakは正常に動き出しました。 カメラに映る情景を簡単な文章にしてしゃべり続ける状況は、私にはそれっぽく人の相手をしているペッパー君やロボホンより人工知能っぽく感じる事ができました。 ただ、むやみにしゃべり続けるのはバカっぽいのでＵＩにもう少し工夫が要りますね。
■２０１６年８月２２日■
しばらくぶりの更新になります。
前回の更新では、、歩いたところまでか。。
歩くための基本的な作りができました。 でも、パラメータ、特に重心高さを変えるとリカッチ方程式の数値解から求める予見制御ゲインも変わってくるため、現在の様に予見制御ゲインをハードコーティングしていては使い物になりません。
ということで、DLLからOctaveの呼び出しと計算結果の取り込み、その前にロボットの姿勢による重心位置の計算をする必要があります。
重心位置はGoSimulation!から得ることもできるのですが、この後ZMPの算出もする関係でどうせ必要になるものなので作る事にします。
ロボットの重心を計算するには各関節部位毎の重心位置をあらかじめ調べておき、関節角度から部位の空間位置を計算し、全質点からのモーメントを合成することで計算します。
普通のロボット構造だと関節が数珠つなぎに構成されているため、比較的簡単なループを回すことで計算できます。この場合、部位毎のデータ構造体には関節軸位置を原点とした重心座標と質量と軸方向を持ちます。座標は関節角度がゼロの状態を基準とします。
そして、先端側から関節角度で重心座標を回転させます。
部位がつながっている部位には関節軸が二つあるわけですから、根本側の関節を原点としたコネクタ座標も持つ必要があります。
そしてローカル座標を根本側の部位に変換しながら関節角度で座標を回転させていき、最終的にロボット全体の原点から見た質点の座標を得る事ができます。
単純な数珠つなぎだと二重ループを回すことで
先端
→→→→→→
↓ A→B→C→D
↓ B→C→D
↓ C→D
↓ D
根本
このようにすべての部位の根本（原点）からの座標を計算できます。
ところが今回のロボットはガッツリとリンク構造をとっているためツリー型構造となってしまい、ちょっと面倒です。
今回のラムダの場合
胴体 ⇒ 股関節ロールブロック ⇒ ベースブロック → 大腿 → 下腿 → 足首ロールブロック ⇒ 足（フット）
→ エレベータブロック ⇒ エレベータリンク → （下腿途中にリンク）
→ ストライドブロック ⇒ ストライドリンク → （下腿途中にリンク）
⇒ 平行リンク → （フットにリンク）
⇒ 能動関節 → 受動関節
このような構造となります。
初代ラムダは単純な数珠つなぎ構造だったので上記の2重ループにしました。
２代目ラムダはマーキュリー足を使った平行リンクです。これは平行だったので、関節数は多いけど関節角度は少なかった事もあり、仮想部位（質量ゼロの部位）を作って処理しました。今回の構造で表現すると
胴体 ⇒ 股関節ロールブロック ⇒ ベースブロック → 大腿 → 下腿 → 足首ロールブロック ⇒ 足（フット）
⇒（股関節ロールブロック）⇒（ベースブロック）→ エレベータブロック ⇒ エレベータリンク → （下腿途中にリンク）
⇒（股関節ロールブロック）⇒（ベースブロック）→ ストライドブロック ⇒ ストライドリンク → （下腿途中にリンク）
⇒（股関節ロールブロック）⇒（ベースブロック）⇒ 平行リンク → （フットにリンク）
こんな感じです。股関節ロールブロックとベースブロックは仮想部位を作って実際以上の部位数のデータ構造を持たせることで単純リンクに変換しました。
今回もそうしても良かったのですが、ベースブロックから接続している４部位の枝はそれぞれベースブロックの別の位置に軸があるため、ベースブロックだけでコネクタ座標を４つ持つ必要があります。他のブロックは１つだけで良いのに。。
どうするか。。
案１ コネクタ座標は最大数の４つ持つ → ○簡単 ×汎用性がない ×メモリーの無駄 → 趣味じゃない → ボツ
案２ コネクタ座標をリスト管理 → ○汎用性あり △ちょっと複雑 → アリ
案３ コネクタ座標を子供に持たせる → ○汎用性あり ×複雑（決定後判明したのだが） → アリ
ここでもう一つ検討すべきことが。
今回構造は上の図でも判るように受動関節がたくさんあります。 能動関節を決定したことで決定するのが受動関節です。
リンク構造だとキネティックス計算は順方向も逆方向も幾何計算を多用することになります。（主に２円の交点を求める）
すると、受動関節の角度を計算するための途中にベースブロックを基準とした各部位の空間座標と姿勢が求められてしまうのです。
データ構造を簡単にするために一度計算した結果を捨てて、再度別方法で計算するってのはあまりにナンセンスで趣味じゃないってことで、案２もイマイチとなりました。
結果、案３で行く事にするとデータ構造はこのようになります。
胴体 ⇒ 股関節ロールブロック ⇒ ベースブロック → 大腿
→ 下腿
→ エレベータブロック
→ エレベータリンク
→ ストライドブロック
→ ストライドリンク
→ 足首ロールブロック ⇒ 足（フット）
⇒ 能動関節 →可動コネクタ軸
大腿から足首ロールブロックまではベースブロックを基準に、エレベータサーボ、ストライドサーボで位置と姿勢が決まります。
正確には足首ロールブロックの姿勢はベースブロックにある足首ロールサーボで決まるし、大腿、エレベータブロック、ストライドブロックの位置はベースブロックに対して固定ですが。
可動コネクタ軸という概念を取り込むことで計算量を最小にしつつ汎用性を維持し、無駄なメモリーを消費しないデータ構造としました。
可動コネクタ軸って、べた計算と一緒やんという見方もあるのですが、多分腕は単純な数珠つなぎ構造になるので、単純構造でも記述が少なくなる方法が欲しかったんです。
できました。 途中それなりにデバッグに苦しんだけどそこは割愛。
グラフは、計算したもの（青）と、GoSimulationから得られる重心位置（オレンジ）です。 歩かせるとスリップやらなんやらで少し斜めに歩いてしまうため、オレンジ色は補正しています。 上は単純に直流除去。下は斜めに歩いているという前提で座標回転させてみました。 まぁ一致してるっぽいですね。実はGoSimulationとプログラム上の質点モデルが若干違うので計算が合っていても完全には一致しません。
ちなみにこれは静止重心を歩いている一コマ一コマの姿勢に対して算出したものです。ZMP計算とは基本的に異なるものです。
最初、どうしても計算が合わなかったのだけど、よく考えるとGoSimulationからの値はグローバル座標。自分で計算したのはロボットローカル座標なので合わなくて当たり前でした。空間座標に換算すると上記のようになりました。
さて、質点モデルと、その巡回方法が確立したので次はZMPの算出です。
たかが重心計算に色々と細かい検討をしてきたのは、ZMP計算は重心計算よりも計算量が大きくなる上に動力学フィルターを組み込むとなると１コマ辺り40回（予見制御未来項の数）計算せねばならないためできればコンパクトにしたいという事があるためです。（いや、どちらかというと趣味だからって方が強いかも（笑））
重心計算の際に慎重に検討したので、注意しながら以前に作ったZMP算出関数を、新しいデータ構造に書き換えて、、比較的穏便に、出来ました！
１：目標ZMP ２：重心軌道からのZMP ３：重心軌道 ４：多質点モデルからのZMP
今の構造だと、ロボット原点に対して重心が少しだけ後ろ（12ミリほど）にあるんですね。足裏がデカイのでそのままでも（GoSimulation!上では）歩くんですが、せっかく重心計算までやってるんだから補正しとくか、と思って間抜けなことがありました。
ロボットの下半身姿勢構造体というのを作っていて、その中に重心位置補正のパラメータを持っているので、歩行の基本姿勢時の重心を計算し、その結果を補正パラメータに入れることで勝手に補正するという事をやってます。
12ミリずれているからといって12ミリ補正してもジャストにはならないのですが、何回か繰り返せばほぼジャストになるだろうと思ったら、17回繰り返したらほぼジャストになりました。 でも、思いっきり足を前に出した姿勢になってまともに立てない姿勢に。。（泣）
当たり前のことで、重心計算結果を原点に近づけるには背中が重い分だけ足を前に出す事になるに決まってます。
やりたかったのは補正値と重心計算結果を一致させること。
どうも頭がドメスティックになってるようでグローバルに切り替わらないようです。(>_<)
補正した結果、目標ZMPへの一致具合がすこしよくなりました。 見た目はかわんないですね。
１：目標ZMP ２：重心軌道からのZMP ３：重心軌道 ４：多質点モデルからのZMP ５：GoSimulationからのZMP
あと、GoSimulation!の機能で、重心とZMPを表示させることができます。 表示設定で歩かせたのがこれです。
こうやって見るとZMPが支持脚に移動していく様子がよくわかります。
蹴り足は加速、迎え足は減速移動することで支持脚にZMPを保ちながら歩くというのは、数式的には理解できても、なかなか実感しにくいですが、こうして視覚的に見ると理解しやすいです
さて、次は動力学フィルターの実装です。
材料は揃ってるはずなので意外と簡単にできちゃうんじゃないかと思っていたんだけど、、
ええと、予見項のZMP計算をするのに、予見項分のモーションを生成して、、はっ！ダメやん。
今のモーション生成は目標ZMP列を参照し、前回までの状態を保持しておいてそれを利用して姿勢を生成するという作りになっています。
予見計算するたびに40コマ姿勢計算して、予見計算をした結果を反映した後、40コマ戻ってから1コマを生成するということになります。戻るのか。。
保持するべき変数を一度退避して再度セットできるようにしないとならないわけですか。そうですか。。
■２０１６年９月５日■
では動力学フィルターの実装です。
なんとなく、現状の予見器の、目標ZMPとの差分を算出する項を多質点モデルから算出したZMPを使えばいいんじゃないかと思ってやってみましたが、ダメでした。これがうまくいくなら、そのままセンサーから取得したZMPを入力すればいいやん、と思ってたけどそんな簡単じゃないですね。
↑ 予見器に入力する、目標ZMPとの差分項を多質点モデルからのZMPにしてみた
やはり教科書通りに実装してみます。 教科書151ページの図4.43を見てください。
まずは単質点モデルでの重心軌道を算出します。 次に予見項分のモーションを生成し、そのモーションを多質点モデルに入力し、ZMP列を得ます。目標ZMPとの差分列が予見項分だけ生成できました。
これを動力学フィルター用の予見器に入力し、この差分ZMPを再現する重心軌道を生成します。
元の重心軌道に差分から生成した重心軌道を加え、最終的な重心軌道とします。
大枠としてはオフライン生成法と同じ方法で目標ZMPに近似させるということになります。 オフライン生成の場合は全工程のモーションを確定してからの再生になっていたのが予見項分だけを先に計算。 更にいうと実行するモーションは再生時に生成することになるので、制御アレンジを加えやすいのではないかと思います。
↑ 左は見慣れた目標と重心軌道と実ZMP 右が動力学フィルターへの入力波形と差分の重心軌道
↑ これは差分の重心軌道から逆算したZMP（単質点モデル）
当初、動力学フィルター用の予見器にはそれ用のリカッチパラメータを用意しなければならないのかと思っていましたが、このグラフを見る限り、同じパラメータ列を使ってよいようです。
動力学フィルターがちゃんと動いているようなのでこの効果を見てみましょう。
胴体に足しかないモデルでは単質点モデルからの差がそれほど顕著ではなく効果が見えにくいので、腕をつけましょう。 そのうちつけなきゃならないとは思っていたし。
できました。
ゲロさんの真似をして、手でバランスを取るというのもやってみたいので少し腕を長くしたいところですが、今回つけているのはラムダ2号の腕です。転倒から起き上がるくらいの機能しかないモノだけど、このロボットだと、それもできないかも。
腕が付いた状態で歩かせてみます。
腕のイニシャル姿勢での歩行です。左右対象に広げているだけなので、不安定になるどころか安定度が増しました。
手を広げた状態というのはロボット全体で見ると質量が分散するため、慣性モーメントは大きくなります。慣性モーメントが大きくなると物体は回転しにくくなるので歩行が若干安定する傾向があります。 この腕は片側で260gくらい。両方でも520gです。総重量3.2キロのロボットでも結構効果があるものです。
ちなみにGoSimulationではブロックの重心は中心に設定されるので厳密な重心点は計算できません。 ラムダ側の計算は重心点をそれぞれ調べて保持しているので若干正確なはずです。比較してないけど自己満足レベルとは思いますが。。
動力学フィルターもできて、オンライン歩容生成は完成です。
本来の目的であるディープラーニングを使った歩行安定化に向けた次のアクションに移りたいですが、最後に動力学フィルターの威力を確かめてみましょう。
前半が動力学フィルター無し、 後半がフィルター有りです。
軽い腕ですが、これくらい振り回すと加速度でZMPは持っていかれるようですね。 歩行に同期した腕の振りをすると、フィルターの効果が見えにくいかなと思って非同期にしました。 同期させる方がめんどくさいですし。 ちなみに「前に倣え」姿勢でも、「ゆっくりした速度での腕の振り回し」でもフィルターなしで問題なく歩けます。
後半、フィルター有りでは腕の振りに対して腰の動きでカウンターを当ててうまく相殺している様子がわかります。
所詮は予定動作を歩容に組み込んでいるだけですが、勝手に安定動作にしてくれる動力学フィルター、すごいです。
これが、目標ZMPと腕の動きを加えた多質点モデルから算出したZMPです。 腕を左右対称に前後に振っているので左右よりも前後動作に影響していることがわかります。
定速回転動作なのでモーメント的には切り返しのタイミングで加速度が発生しますが、前後方向に分解すると正弦波成分となるので正弦波っぽいノイズが乗ってます。（正弦波を2階微分しても正弦波）
これでモーションの生成ができました。次は外乱を考慮した安定歩行への取り組みです。 さて、どこから手を出しますかね。
■２０１６年９月２０日■
「ヒューマノイド・ロボティクス2016夏の学校」に行ってきました。
GoSimの杉浦さんに、ロボット学会のMLで募集していたこの勉強会に誘ってもらい、学会会員縛りなし、会費なし、ということなので恐る恐るですが行ってきました。
実は主旨としては最新のロボット技術を検討討議する会なので相当な力がないと参加できないもので、参加に対しては予習をしてくるようにと、論文集を指定されていました。
講義１．２．人型ロボットの運動学と力学１，２ 大阪大学 杉原先生
ロボットを力学的に表現するための基本的な方程式について主に説明されました。 杉原先生はこの会の言い出しっぺだそうです。
基本的な状態方程式については梶田先生の教科書にあった部分が基本となっていたのですが、より厳密性を帯びた表現についての解説がありました。
参加の前提として、浮遊系、劣駆動系というキーワードがあって、ヒューマノイドなんだから基本的には歩行なわけで、グリップ歩行であれば路面との接触はバネ・ダンパモデルで表現するのが普通だと思っていたのですが、スリップやドリフト、回れ右のような動作は浮遊系で表現するというような記事を見つけたのでそうなんだーと思っていたのですが、浮遊系は宇宙ロボットの話でした。
浮遊系のダイナミクスとしては反動零空間というのがあって、運動量保存則や角運動保存則が働くと言った話や、地盤（地面）をどう数学的に表現するか、硬いままでいいの？とか、運動方程式を解くために正則化が必要でその工夫をしているのだけど果たしてそれでよいのだろうか？とか、非線形ソルバーを使うっていう風潮になってきたけどそれっていいんだっけ？とか、数学表現の厳密さからロボットの挙動を解明しようとしている様がよく見えました。
スラッグ変数とかNULLスペースとか聞きなれない単語が多発。 あとノルムってのもよく理解できてない。 「それはノルムですか？」 「ノルムです。」 ってのがマイブームです。使えるようになりたい。
あとは速度ベースがよいのか加速度ベースがよいのかとか言われていました。 運動量次元で解くか力次元で解くかという話ですが、加速度で取り扱うと精度の取り扱いが難しくなるという話だったと思います。（すいません。これは他の講義の話と混同しているかも）
杉原先生は理論派の急先鋒だそうです。 理論派以外ってなに派なんだろうな。
講義３．人型ロボットの制御１ 東京大学 山本先生
山本先生は１日目の夜に催された懇親会の幹事をされていたんですが、私が飛び入りで参加させてくれとか言いましてご迷惑をおかけしました。どうもすいませんでした。
山本先生は低次元力学に基づく制御ということで倒立振子のダイナミクスで表現される歩行のメカニズムとそのとらえ方について説明されました。 この辺は梶田先生の教科書にも近いところがあったのですが、より制御的な表現をされたので、逆に制御の理解が少し深まりました。
その上で歩行を理論的に考える術としてキャプチャーポイントという概念の説明をされました。 そういったある意味原始的な歩行の理論からZMP表現への進化？ さらには最適制御などの変遷、そして足裏を持ったロボットの安定条件モデルの説明をされました。
山本先生が説明されていた「MOA集合」に使われていた安定表現のグラフ？（これはなんていうのでしょうか？）を探していたら、山本先生の論文が出てきました。
最後にポポビッチだったかな？メモるの忘れてたけどヤバい（山本先生談）シミュレーション歩行の紹介をされていました。 でも、シミュレーションって正体不明だからヤバいかどうかわかんないですよ。(^O^) 名前忘れちゃったから検索できない。
講義４．人型ロボットの制御２ 東京大学 神永先生
ロボットにおける駆動系の話。
ロボットの関節制御では位置制御だけでは足りなくて力制御が必要。 その中にバックドライバビリティがあります。 外力に倣うってことです。
これは結構大変で、モーターのトルクを抜いていてもギアの抵抗やイナーシャがあって十分なバックドライバビリティが得られないため、いろんな制御系が考えられているというお話。
神永先生は油圧派のようで、作っているロボットもフルスクラッチの油圧アクチュエータで構成されているそうで、この日の講義後に東大の研究室見学で見せてもらえました。
駆動系の制御に必要なトルクセンサーの話や外乱オブザーバーによる推定外乱フィードフォワード制御について解説してもらいました。
講義５．人型ロボットの状態推定 九州大学 舛屋先生
状態推定ということで、センサー値に含まれる外乱・ノイズを考慮してどのように真値を推定するかの話。 相補フィルター、カルマンフィルターおよびカルマンフィルターの派生技術について。
外界とロボットの関係が未知な状態で、どのようにロボットの状態を推定するか。ロボットが何らかの形で外界と接触していることを前提にその点を起点に状態推定をする考え方についてのお話でした。
舛屋先生はちょっと気の弱い感じの方で、杉原先生に議論をふっかけられて困っている姿が印象的でした。 がんばれ！舛屋先生！
講義６．人型ロボットの運動計画 神戸大学 田崎先生
歩行運動の計画についてです。
人型ロボットの運動には接触、重心運動、全身運動という要素がそれぞれ関連しあっています。 これを成立させながら目的にそった歩行運動を成立させる計画を立てる必要があります。
もちろん、途中で環境が変わることも見越して逐次計画を更新しながら、そして全域に渡って歩行運動が成立する解を実時間で解くための学問です。
これも平坦な道を歩くだけなら比較的簡単そうですが、全身運動で乗り越える必要がある段差とか、横歩きしないと通れない隙間とかも含めて効率的に解を導く方法の基本と最前線の紹介をしてもらいました。
田崎先生は制御の出身だそうで、ロボットってロボット学会があるんだけど、自動制御学会でもロボットっぽい研究をしていたりして、制御の要素は非常に強いです。
でも、ロボットやってる人は意外と制御に縁遠い人も多いとかって話で、田崎先生は学生の勉強不足を嘆いていました。
線形リカッチまでは必須。 そしてそれが限界でしょう（杉原先生談）ということです。 多分田崎先生は非線形リカッチも勉強してほしいと思ってるはずです。
なんとなく、杉原先生と論戦を交わせるのは田崎先生だけなんじゃないかな？とか思わせる口ぶりで粛々と講義を進められてました。 アンニュイです。
講義７．ヒューマノイドロボットの全身制御とマニピュレーション制御 立命館大学 小澤先生
講義の最中にはいつでもインターラプトＯＫで、質問、議論をふっかけることができるのですが、学生風情にはなかなか難しくって、講演メンバー＋何人かのオーソリティがその役をになっています。
小澤先生はちょっとドスの聞いた口調で質問疑問をバシバシ投げかけてた方で、注目していたのですが、講義７で登場です。
小澤先生はマニピュレータの研究をずっとされているそうで、マニピュレータ制御と歩行制御での考え方の違いやマニピュレーションでよく考えるインピーダンスについての基本的な説明などをされました。
ヒューマノイドのバランス安定化問題というのをドスンと放り込んできて、しばらく討議がされましたが、ロボットの安定表現についてはまだ議論中ということでひとまず落ち着きました。
その話の中で、リアプノフの安定性ってのが出てきて安定性表現はかくあるべきじゃないのか？というような話になったのですが、動力学フィルターの検討しているときにいろいろ文献を調べたりするとリアプノフの安定性ってよく出てきました。 さっぱりわからんかったのですが、この話でちょっと言ってることが分かった気がしました。 もう一度勉強してみよう。
講義８．人型ロボットの制御４ 早稲田大学 橋本先生
日本のロボットの聖地である早稲田大学の先生の発表だけあって、実際のロボットの動画がたくさん出てきてほのぼのした講義でした。
主に機構系の話で、特に最近のアクチュエータ事情について、足部の構造についていろいろな事例を紹介してもらえました。
私もハーモニックドライブ使いたいなーって思ったんですけど高いんですよね〜(>_<)
最近のロボット用？アクチュエータはモーターからして部品で売っていて、フレームはロボットの設計に含まれるのが通例となっているようです。ホビーロボットもそんな感じにしたらもっとスタイリッシュなロボットが増えたりしませんかね。設計・製作の敷居が上がるか。
特別講演 梶田先生
最後は梶田先生のセミナーです。
梶田先生は主にDARPAに参加された話を中心に、ＭＩＴのアトラスに実装されたスタビライザーの紹介をされました。
他の講義もとても面白かったのですが、このＭＩＴの新型スタビライザーの紹介が一番興奮しました。 梶田先生が論文を説明用に簡略化させた数式と図で概要を説明され、結局よくわからんかったのですが、帰って今日、すぐにネットを探して論文をダウンロードしました。英語なのでまだ読めてませんが、ちょっとちゃんと読んでみようと思います。
見つけた論文は梶田先生が言ってたのとは違っていたので、その後梶田先生に教えてもらって正しいのをゲットしました。
論文情報です。「Optimization-based locomotion planning, estimation, and control design for the atlas humanoid robot」
とにかく楽しい二日間でした。 次回もし開催されたらまた参加したいですね。 ロボット学会また入ろうかな。
フェイスブックにロボティクス夏の勉強会に出て思った事を書いたら思ったより反応があったのでこちらにも載せておきます。
反応があったのはおまけで書いた最後の部分です。転載じゃなくて加筆修正しています。
多分、想定より参加者が増えたので主催者側で講演の技術レベルを下げたとは思うのですが、討議している研究者の方ほど理解できてはいないにしても、話の内容にはついていけた事。よかった(^^)。
これは梶田先生の教科書のおかげです。あの本をマスターすれば受講生としてはなんとかなる。すごい本だなーと改めて思いました。
正確には梶田先生の教科書も最後の方は難しい上に説明が少ないのですが、ロボットを動かそうとしている身としては最後の実効的な部分こそが知りたいところなので、教科書にあるキーワードを頼りに結構調べてまわったりはしました。 それが役に立ってる。
次に数学。
勉強会後に飲みに行った際に、「今日の会は主催者の嗜好があるから理論系が強くて、、、」という話にはなりましたが、数式で意思疎通できる、とまで言わなくても数学頑張らなきゃダメです。論文が読めません。
数学といっても範囲は限られてるので頑張ればなんとかなると思います。自分も含めて。
私は理論研究はまだまだ無理なので実装ベースでロボットの実現を目指すのだけど、それには先駆者の知見が必要でそれは論文にあります。
あと、英語。
英語読めないと論文が読めません。英語頑張りましょう。
今回の勉強会の事前学習に論文集が提示されたのですが、全部英語でした。数式があるからなんとかナニについて書いてるのか想像つくものもありましたが、予習できたとは言えませんでした。
最後に、講義を聴きながらフロスティさんと話していたのは実機実装のネタが少なかった事。隠してるのかもしれないけど理論、シミュレーションが多くて、実機実装したらまだまだ問題出るだろうなと思われるものも多々ありました。
それで私が思ったのは、小型ロボット作ればいいのに、ということ。
等身大以上がベースで研究してる方が多くて、もちろん実用とか意義とか考えると実物大でなければならないのだけど、理論整備してるんだからスケールさせるとか、理論で処理するとかなんとかなると思うんですね。
小型なら安いしこけても壊れにくいし、小型でうまくいったら実物大作ればいいと思うのです。
小型といってもロボワンサイズは小さすぎて機構実装できないので1メートル弱くらいかな。
最後のはおまけですが、そんな感じです。
この主旨のフェイスブックへの投稿に対して、細田先生からコメントをいただきました。 やはりスケール問題で、課題とすべき理論がうまくいったりうまくいかなかったりしてしまうということでした。
研究者の間でもそのような議論がされているということです。
理論研究をしている段階ではやはり条件というのは大事な話で、それを欠くと何やってるのかわからなくなるというのはもっともなことです。
でも、一方、ロボットを実用化するときには結構複雑なシステムとして扱わなければならなく、様々な予期しない問題が発生するもので、ミニチュアとは言え、「完成体」を作る事は大きな意味があると思います。
昼休みの雑談で、梶田先生が「デモの準備にかかる時間が膨大で困る」といった事を言われて、「ロボでサバゲ」をやっていた時の事を思い出しました。
私は大会に出場しないので「デモ」したり「試合」したりという本番がないのですが、「ロボでサバゲ」は直視禁止の遠隔システムで、被弾センサー含めて「完成体」とその運用を要求されました。
操縦とロボットのモニターと画像通信の３系統通信、バッテリー管理とか被弾装甲の交換とか、センサー調整とか、BB弾発射のノイズでCPUがリセットするとか(>_<) 結構忙しくて、「これはロボットシステムのミニチュアだな」と思いました。