玉乗りロボットをシミュレートしてみた

［｜　］　 最終更新： 2011/04/17 20:08:12

はじめに

OpenHRP3を使う練習のために、玉乗りロボットのシミュレーションを試みました。

OpenHRPの本来の開発目的である、ヒューマノイドとはかなり毛色が違う対象で、ちゃんとシミュレーションできるのかは不安ですが、「ちゃんとシミュレートできている」かどうかの判断は「実機の性質」を知らなければ、できません。
ヒューマノイドロボットは複雑で、かつ、静特性はともかく動特性はぱっと見ででは妥当性がわかりません。
一方、玉乗りロボットは基本的な特性は十分に把握、制御ゲインで問題があったときにも、挙動から何が起きているかを検討付けやすいので、「知っている対象だから」という理由で選定しました。

それがいろいろと難儀を起こすことになったのですが...。

シミュレーション対象

対象は玉乗りロボットBallIPです。 詳しくは富士技術出版のDownloadで公開されています。
ただし、このロボットから持ってきたのは

のみです。大きく異なるところは です。それは全く違うものになりそうですが、本体と球のパラメータが挙動としては重要なので、あまり気にしませんでした。 逆に、動特性を決める、慣性パラメータを「現実離れしない値」にするために、BallIPをネタにしました。
OpenHRPは、可動部分をトルク(力)操作型(torque、トルク)と、角度(変位)操作型(highgain、ハイゲイン)で指定できます。 ところが、ハイゲインモードはロボット全体の構造の一部で角度がかわるようなところでは問題なさそうな感じなのですが、接触駆動部のようなところで、角度(および角速度、各加速度)を指定すると、接触状況にかかわらず、ただ回って摩擦力の伝動がうまくいかない、という現象にあたりました。球にローラを押しつけて、ローラの角度を回しても、ローラの角度が変わるだけで、球の角度には影響が出ませんでした。
そのため、加速度操作型ではなく、トルク操作型にしました。

シミュレーション結果

OpenHRP用プロジェクト一式とコントローラ一式

ZIPダウンロード (2011/04/16, 43,031 bytes)　 同ファイル群

シミュレーション結果



モデル化:GrxUI

作成したモデルの基本概念

モデルのワイヤフレーム
モデルのワイヤフレーム
モデルのソリッド
モデルのソリッド
玉乗りロボットの要素は、言うまでもなく、球とロボットです。
モデル：

本来、この構造は、アクチュエータ付ローラを４本回すので４自由度のモデルになるのですが、ローラの位置をロボットボディに対して固定したモデルでは安定した接触が得られませんでした(拘束条件考えれば接触点数が多く、かつ球のモデルの凹凸で隙間があいたりはねたりする)。 そのため、球とローラの接触を安定させるために、制御とは無関係にローラを押しつける方向に直動関節を作って、一定の力で押しつけることで、接触力の安定化を図ることにしました。

モデルの詳細：板と球

この球の形状モデルは、自作のperlスクリプトicosa.plで、正２０面体の各面を1/3内挿(１枚→９枚)→1/2内挿(１枚→４枚)で細かくした上で、各頂点の方向を変えずに半径0.11に正規化してつくっています。 合計720ポリゴンです。
（標準出力に吐くので＞でファイルに）

技術的に細かい補足：


モデルの詳細：ロボット(モデル名robot_body)

ロボットのモデルツリー
ロボットのモデルツリー
root座標系(初期)の設定
root座標系(初期)の設定
直動軸の設定
直動軸の設定
ローラ軸の設定
ローラ軸の設定
右図にロボットのモデル構造を示します。
モデル(robot.wrl、ロボット、 _cylinder.wrlをInline)
ロボットボディがrootで、そこに４セット(X1,X2,Y1,Y2)のローラが付いています。
各ローラはローラを球に押しつけるための直動関節S(XY)と、ローラの回転軸R(XY)でできています。

ロボットボディは

としています。
それぞれのローラ部分は、 なお、モデルのファイルはテキストなので、ローラのセットを増やすところはテキストエディタでコピペしました。 たぶん、GrxUIで設定していくより楽です。(編集→保存→GrxUIで再読込)

コリジョン設定

シミュレーションにおいて、「接触して動作する部分」にはコリジョン設定が必要です。 そのため、「床～球」「球～ロボット」が最低限必要です。 「ロボット～床」は「正しく制御されていれば」不要な設定ですが、ないと、１：ロボットが転ぶと床を突き抜けて落ちていく(無害ですが)　２：ロボットを裏返して、駆動系のチェックをするときはロボットが床におけたほうがいい　という理由で設定しています。

シミュレーション設定

とりあえず、デフォルトのまま、時間だけ変えました。



コントローラ

OpenHRPのコントローラの概要

OpenHRPはシミュレーションに係わる演算を複数のプログラムに分けて実装してあります。 そのため、複数のコンピュータに分散させたり、マルチコアのCPUの場合にスピードアップする効果があります。
それぞれの間はOpenHRPの仕様で通信されていますが、そこに「コントローラブリッジ」を置くことで、シミュレーションの状態量や、対象に対する操作入力を、ＲＴミドルウエアの形式で出し入れできるようになります。

そのため、OpenHRPで制御系を作る場合は、

をします。ブリッジ－制御系の間のやりとり、センサ情報やアクチュエータ指令などを、実機(とつながる低レベル処理プログラム)－制御系と互換にしておけば、実機とシミュレーションをつなぎ替えて検証することができるようになるわけです。 原理的には、制御側を単一のコンポーネントにする必要はなく、複数のコンポーネント群でもＯＫなはずです。

コントローラブリッジ

詳しい使い方、仕様はコントローラブリッジマニュアルに記載があるので、実際に使った玉乗りロボットのブリッジの起動設定を以下に示します。 これをバッチファイルにしておき、OpenHRPに設定しておくと、シミュレーション開始時に自動的に起動し、別に動かしておく制御コンポーネントと接続して、シミュレーションが開始されます。 

"%OPENHRP_SDK_ROOT%\bin\openhrp-controller-bridge" ^  ブリッジの実行ファイル
--server-name BBcontroller ^                          ブリッジのコンポーネント名
--out-port acgyro:RATE_GYRO_SENSOR ^                  ジャイロの出力をacgyroポートで出力
--out-port acaccel:ACCELERATION_SENSOR ^              加速度センサの出力をacaccelポートで出力
--out-port acjoint:JOINT_VALUE ^                      関節変位(スライダ位置、ローラ回転)をacjointポートで出力
--in-port cmforce:JOINT_TORQUE ^                      関節トルク・推力をcmforceポートで入力
--in-port cmjoint:SX1,SX2,SY1,SY2:JOINT_VALUE ^       スライダの関節位置をcmjointポートで入力
--in-port cmspeed:SX1,SX2,SY1,SY2:JOINT_VELOCITY ^    スライダの関節速度をcmjointポートで入力
--in-port cmaccel:SX1,SX2,SY1,SY2:JOINT_ACCELERATION ^スライダの関節加速度をcmjointポートで入力
--connection acaccel:BBcont0:acaccel ^                以下、制御コンポーネントBBcont0のポートと、
--connection acgyro:BBcont0:acgyro ^                        ブリッジ起動時に自動的に接続
--connection acjoint:BBcont0:acjoint ^
--connection cmjoint:BBcont0:cmjoint ^
--connection cmspeed:BBcont0:cmspeed ^
--connection cmaccel:BBcont0:cmaccel ^
--connection cmforce:BBcont0:cmforce
※「^」は複数行にコマンドを書いて連結する記号。
※%OPENHRP_SDK_ROOT%は自動設定されるインストールディレクトリの環境変数
※「出力」「入力」はコントローラブリッジ側から見て。制御コンポーネントにとっては「入力」「出力」になる。
※個人的な規約で、「実際状態量」は「ac～」、「操作指令」は「cm～」で命名

すべてのポートは、TimedDoubleSeq型のデータ形式で、「時刻＋各値(double)の配列」という形です。 ジャイロと加速度センサは各１個で各々３軸なので、各々３要素の配列、ロボットは８関節(４ローラ＋４スライダ)あるのでacjointとcmforceは８要素の配列になります。値の順番は標準で関節に設定した番号順になります。
一方、cmjoint,cmspeed,cmaccelは中間に「:SX1,SX2,SY1,SY2:」と記載してあるので、４つのスライダの値に限定したもので、ローラは無関係になります。 スライダはIDが４～７でふってありますが、関節を限定すると、ここに記載した順番の配列になります。つまり、要素０がSX1、要素３がSY2の値になる４要素の配列です。

最後の--connectionによって、ブリッジが起動したときに、すでに動いているコンポーネントのポートと接続します。 なお、「--connection acaccel:BBcont0:acaccel」は「[ブリッジのacaccel]:[BBcont0:(の)acaccel]」という解釈です。 入出力の方向は関係なく、ブリッジ側：制御側、の順です。

ブリッジの起動は、GrxUIの「コントローラ」で設定します。バッチファイルを置いたところを実行ディレクトリにして、実行コマンドとしてバッチファイル名を指定します。コントローラ名は--server-nameで指定した名前です。 なお、このディレクトリにはブリッジの(かなり巨大な)ログが気づくと大量にたまります。 また、このディレクトリは、「プロジェクトのxmlファイルに対するの相対パス」で扱われるようです。 遠いディレクトリにおくと「../」が大量に続いたり、異なるドライブに分散させたりするとおかしくなるようです。 同一ディレクトリにシミュレーションモデルとコントローラのソースを置くのもどうかと思いますので、隣のディレクトリくらいにしておくといいかもしれません。もしくは、ブリッジのバッチファイルをモデルと同一ディレクトリにしておくか。

なお、このバッチファイルにエラーがあると、理由も分からずにシミュレーションの起動に失敗します。 おかしいと思ったら、コマンドプロンプトでそのままこのバッチファイルを実行して、エラーが出ないことを確認したほうがいいでしょう。

制御コンポーネント

基本的には、ＲＴミドルウエアのコンポーネントの開発に従います。
コントローラ作成ガイドにあるように、rtc-templateを用いると、VC++でビルド可能なソリューション一式ができます。
テンプレートでできたものをただビルド、実行しても、動作するコンポーネントはできます(もちろん、なにもしません)。

あとは、この中に、制御のプログラムを書き込みます。最低限、実装すべき関数は

だと思われます。

OnExecute関数の冒頭に while(1)で囲まれた、値の取得部分があります。 本来は、単にisNewしてreadすればよさそうなのですが、書き方によっては、前回のシミュレーションの最後の状態が読めたり、不思議な現象が再現性良く現れたので、その対策として、時刻０が読めるまではから読みするようにしています。

コントローラ開発の手順

コントローラの開発(実行、デバッグ)のルーチンワーク：

  1. GrxUIを起動、プロジェクトを読み込む。
  2. モデルの修正をしたら、モデルの保存。モデルの初期状態などの修正は、プロジェクトの保存の必要もあり。 そのたびに、再読込をしておいたほうがいい。
  3. VC++でコントローラの.slnを読み込み。Ｆ５でビルド実行。
  4. シミュレーション実行。
  5. 修正する場合は、シミュレーション終了後・止めてから、コントローラを[×]。 修正、再実行、シミュレーション開始。「再スタートしますか」には「はい」を選択したほうが、たぶんいい。
※printfデバッグは便利。コマンドプロンプトはプロパティで画面サイズ、バッファサイズ(スクロールバーでうごける)を変更できる。→「同じタイトルのウインドウに適用」、しておくと次に実行したときにそのサイズで起動。
※GrxUIの３Ｄビューを出したままにしておくと、シミュレーション速度が落ちるので注意。

入出力の追加

最初は、コントローラ作成ガイドにあるように、rtc-templateをつかって、ひな形を作りました。 そのあと、上記のようにかなりの入出力を追加していますが、そのたびにrtc-templateで作り直したわけではありません。 rtc-templateでつくられたソースはわかりやすく、ヘッダファイル(今回はBBcont.h)とコード(BBcont.cpp)の該当箇所

という手順で、増やしたり減らしたりしました。もちろん、そのつど、ブリッジ起動バッチファイル BBcont.batの--in(out)-portと--connectionを設定します。

制御シミュレーション

制御方法

ロボットの制御プログラムは、

が主要部分です。

姿勢角度の計算は、加速度出力とレートジャイロの出力から計算しています。 加速度は重力加速度方向をある程度検出できる一方で、ロボットの振動の影響を受けます。特に、OpenHRPの加速度センサは帯域に制限がないようで、かなり過激な波形がでます(実際の加速度センサは、センサの帯域や取り付け方法などによってほどほどになまっている)。そのまま使えません(本来は、水平成分と垂直成分をatan2にいれれば何とかなりそうですが垂直成分も変動が大きいので、9.8決め打ちにしています)。
ジャイロはそのまま積分すると、動き方によっては角度にずれが出ます。実在のレートジャイロに比べると、ドリフトしないのは理想的ですが。 そこで、この二つを、実在のBallIP同様に合成ジャイロの手法で、合成して、傾斜角度を求めています。 具体的には、加速度センサから計算した傾斜角から低域(ほぼ直流分)を取り出し、ジャイロの積分値から広域を取り出し、合算しています。

球の転動量はローラの回転角度で代用しています。ローラと球の間にはすべりが明らかにあり、同方向に回転する２本のローラは期待では同一回転をするはずですが、明らかに異なる角度で回っています。 ので、ここでは、単純に、同方向の２本のローラの回転の平均値を使いました。

制御、DoControl()では傾斜角、角速度、球移動位置、移動速度に対して、K1～K4のゲインでＰＤ制御をかけています。
なお、ゲインは、実験的？に決めたので、根拠はありません。
また、制御に味をだす？ため、目標値を時間とともに振るようなデモコードが入れてあります。

ローラ押しつけの制御：危険な小細工

ローラの回転を伝えるには、ローラを球に押しつけないと摩擦力が不足します。 これを固定の構造では、上述の通りシミュレーションがうまくいかなかったため、スライダを加えて、一定の力で押しつけるようにしました。 ただし、単に押しつけ力を発生させるのでは、二つの問題があります。
一つは物理的に明らかな現実にもある問題で、力でおすだけだと、微妙な力のずれや外力などでペアのローラが一緒に原点からずれていくという現象です(結果、ロボットと球がずれる)。本来はこの対策はまじめに実装する必要がありますが、もう一つの影響が大きいので、あわせて対処しています。
二つ目はローラのめり込み現象です。ただ、押しつけるだけなら問題なくとも、その状態でローラをまわすと、ローラがどんどんめり込んでいく現象が見られました。原因は不明ですが、おそらく、接触力の計算値が、ぎりぎり接触を維持するのに不足して、僅かずつ沈んでいく、という感じなのではないかと思います。

この現象に対処するために、いくつか試して現状で採用した方法は、「推力を設定しつつ、位置を強制する」です。 現行のシミュレータ(コントローラブリッジ)では、ハイゲインではない、トルク(力)モードであっても、位置、速度、加速度を設定できるようです。 具体的には、ブリッジのin-portで、TORQUEだけではなく、VALUE、VELOCITY、ACCELERATION を設定し、そこに制御側から値を書き込みます。 onExecuteの後半に実装してあり、

という処理をしています。
言うまでもなく、この方法は反則です。ロボットシミュレーションを行う上では、現実との乖離しないことに最大の注意を払う必要があって、乖離したらそのシミュレーションは無意味です。現実にはこんな操作はできないので(厳密には静的な力を与えることはできるけど結果の挙動が明らかにおかしい)、この点で、このシミュレーションはアウトです。
ただ、これはロボットの特性の本質(球を回してバランスをとる)とは別の部分なので、それなりにそれっぽい感じで動いている、と考えられます。

バリエーション

球をまわすだけ
球をまわすだけ
単に、球の回る様子を見たいという場合：

１：モデルの変更

すると、右図のような感じになります。

２：制御コンポーネントの修正
BBcont.cpp の DoControl() で、ローラトルクを強制指定。 

        tx=K1*(thxr-thx)+K2*(0-wx)+K3*(xr-px)+K4*(0-pvx);
        ty=K1*(thyr-thy)+K2*(0-wy)+K3*(yr-py)+K4*(0-pvy);

追加    ty=0;
追加    if((ec/100)&1) { tx=0.3; } else { tx=-0.3; }

        Limit(tx,1.0);	
        Limit(ty,1.0);
なお、ただ、回していると、ローラが回らなくなって、そのまま球にめり込むことがあります。 これは私には手の打ちようがありません。


熊谷正朗 [→連絡]
東北学院大学 工学部 機械知能工学科 RDE
［｜　］