SunFounderが販売しているArduinoが搭載された小型四足歩行ロボットです。
制御ボード | SunFounder Nano(ATmega328) |
RCサーボ | EMAX ES08A II 12 個 回転数( 6 V ) :43 rpm 静止トルク( 6 V ) :0.15 Nm |
無線モジュール | nRF24101 |
バッテリ- | 18650 リチウムイオン電池( 3.7 V ) 2 本 |
バッテリーは付属していないため、別途購入する必要があります。
PCとArduinoをUSBケーブルで接続して制御を行います。
開発環境などは以下の通りです。
LEGO Mindsorms EV3はレゴが販売しているセンサやモータ をインテリジェントブロックEV3 と接続してロボット等 のシステムを組み立てることのできるキットです。
インテリジェントブロックEV3が4台、Lモーター8個、Mモーター4個を組み合わせることで四足歩行ロボットを作成できます。
CPU ARM9 | 300 MHz OS ev3dev ( Debian GNU Linux ) |
EV3 Lモータ | 回転数:160~170 rpm 静止トルク:0.21 Nm |
EV3 Mモータ | 回転数:240~250 rpm 静止トルク:0.12 Nm |
ジャイロセンサ | 精度3◦ |
カラーセンサ | 距離1 mm~18 mm |
タッチセンサ | スイッチ可動域4 mm |
超音波センサ | 計測可能範囲:3~250 cm |
PCとEV3を無線LANで接続して制御を行います。
開発環境などは以下の通りです
本ソフトウェアは以下のライブラリを使用しています。
Crawl_Gait_Controllerはクロール歩容により全方向の移動を行うための足先軌道を生成するコンポーネントです。
Crawl_Gait_Controller | ||
InPort | ||
名前 | データ型 | 説明 |
target_velocity | RTC::TimedVelocity2D | 目標速度 |
update_pose | RTC::TimedPose2D | 位置の再設定 |
OutPort | ||
名前 | データ型 | 説明 |
Trajectory | RTC::LeggedRobot | 足先軌道 |
current_pose | RTC::TimedPose2D | 現在位置 |
stability_margin | RTC::TimedDouble | 安定余裕 |
NE_stability_margin | RTC::TimedDouble | NE安定余裕 |
コンフィギュレーションパラメータ | ||
名前 | デフォルト値 | 説明 |
sampling_time | -1 | ステップ幅の設定。0以下の場合は実行周期に設定 |
stride | 0.08 | 歩幅 |
center_pos_x | 0.077 | 脚先基準位置(X軸) |
center_pos_y | 0.096 | 脚先基準位置(Y軸) |
center_pos_z | -0.079 | 脚先基準位置(Z軸) |
min_stability_margin | -0.001 | 最低安定余裕 |
lift_height | 0.05 | 遊脚高さ |
upper_limit_x | 0.04 | 脚基準位置からみた可動範囲上限(X軸) |
upper_limit_y | 0.04 | 脚基準位置からみた可動範囲上限(Y軸) |
upper_limit_z | 0.04 | 脚基準位置からみた可動範囲上限(Z軸) |
lower_limit_x | -0.04 | 脚基準位置からみた可動範囲下限(X軸) |
lower_limit_y | -0.04 | 脚基準位置からみた可動範囲下限(Y軸) |
lower_limit_z | -0.04 | 脚基準位置からみた可動範囲下限(Z軸) |
move_limit | 0 | 脚の可動範囲を制御時に考慮するかの設定 |
Intermittent_Crawl_Gait_Controllerは間歇クロール歩容により全方向の移動を行うための足先軌道を生成するコンポーネントです。
Intermittent_Crawl_Gait_Controller | ||
InPort | ||
名前 | データ型 | 説明 |
target_velocity | RTC::TimedVelocity2D | 目標速度 |
update_pose | RTC::TimedPose2D | 位置の再設定 |
OutPort | ||
名前 | データ型 | 説明 |
Trajectory | RTC::LeggedRobot | 足先軌道 |
current_pose | RTC::TimedPose2D | 現在位置 |
stability_margin | RTC::TimedDouble | 安定余裕 |
NE_stability_margin | RTC::TimedDouble | NE安定余裕 |
コンフィギュレーションパラメータ | ||
名前 | デフォルト値 | 説明 |
sampling_time | -1 | ステップ幅の設定。0以下の場合は実行周期に設定 |
stride | 0.08 | 歩幅 |
center_pos_x | 0.077 | 脚先基準位置(X軸) |
center_pos_y | 0.096 | 脚先基準位置(Y軸) |
center_pos_z | -0.079 | 脚先基準位置(Z軸) |
min_stability_margin | -0.001 | 最低安定余裕 |
side_move_distance | 0.03 | 横方向への移動距離 |
lift_height | 0.05 | 遊脚高さ |
upper_limit_x | 0.04 | 脚基準位置からみた可動範囲上限(X軸) |
upper_limit_y | 0.04 | 脚基準位置からみた可動範囲上限(Y軸) |
upper_limit_z | 0.04 | 脚基準位置からみた可動範囲上限(Z軸) |
lower_limit_x | -0.04 | 脚基準位置からみた可動範囲下限(X軸) |
lower_limit_y | -0.04 | 脚基準位置からみた可動範囲下限(Y軸) |
lower_limit_z | -0.04 | 脚基準位置からみた可動範囲下限(Z軸) |
move_limit | 0 | 脚の可動範囲を制御時に考慮するかの設定 |
Trot_Gait_Controllerはトロット歩容により全方向の移動を行うための足先軌道を生成するコンポーネントです。
Trot_Gait_Controller | ||
InPort | ||
名前 | データ型 | 説明 |
target_velocity | RTC::TimedVelocity2D | 目標速度 |
update_pose | RTC::TimedPose2D | 位置の再設定 |
OutPort | ||
名前 | データ型 | 説明 |
Trajectory | RTC::LeggedRobot | 足先軌道 |
current_pose | RTC::TimedPose2D | 現在位置 |
コンフィギュレーションパラメータ | ||
名前 | デフォルト値 | 説明 |
sampling_time | -1 | ステップ幅の設定。0以下の場合は実行周期に設定 |
stride | 0.08 | 歩幅 |
center_pos_x | 0.077 | 脚先基準位置(X軸) |
center_pos_y | 0.096 | 脚先基準位置(Y軸) |
center_pos_z | -0.079 | 脚先基準位置(Z軸) |
lift_height | 0.05 | 遊脚高さ |
upper_limit_x | 0.04 | 脚基準位置からみた可動範囲上限(X軸) |
upper_limit_y | 0.04 | 脚基準位置からみた可動範囲上限(Y軸) |
upper_limit_z | 0.04 | 脚基準位置からみた可動範囲上限(Z軸) |
lower_limit_x | -0.04 | 脚基準位置からみた可動範囲下限(X軸) |
lower_limit_y | -0.04 | 脚基準位置からみた可動範囲下限(Y軸) |
lower_limit_z | -0.04 | 脚基準位置からみた可動範囲下限(Z軸) |
move_limit | 0 | 脚の可動範囲を制御時に考慮するかの設定 |
目標足先軌道から各関節の目標位置を計算するコンポーネントです。
Foot_Position_Controller | ||
InPort | ||
名前 | データ型 | 説明 |
Trajectory | RTC::LeggedRobot | 足先軌道 |
OutPort | ||
名前 | データ型 | 説明 |
motor_pos | RTC::TimedDoubleSeq | 関節の角度 |
motor_pos_0 | TimedDoubleSeq | 脚0の関節の角度 |
motor_pos_1 | TimedDoubleSeq | 脚1の関節の角度 |
motor_pos_2 | TimedDoubleSeq | 脚2の関節の角度 |
motor_pos_3 | TimedDoubleSeq | 脚3の関節の角度 |
コンフィギュレーションパラメータ | ||
名前 | デフォルト値 | 説明 |
offset_lf0 | 0 | 関節のオフセット(左前0番目) |
offset_lf1 | 0 | 関節のオフセット(左前1番目) |
offset_lf2 | 0 | 関節のオフセット(左前2番目) |
offset_lb0 | 0 | 関節のオフセット(左後0番目) |
offset_lb1 | 0 | 関節のオフセット(左後1番目) |
offset_lb2 | 0 | 関節のオフセット(左後2番目) |
offset_rb0 | 0 | 関節のオフセット(右後0番目) |
offset_rb1 | 0 | 関節のオフセット(右後1番目) |
offset_rb2 | 0 | 関節のオフセット(右後2番目) |
offset_rf0 | 0 | 関節のオフセット(右前0番目) |
offset_rf1 | 0 | 関節のオフセット(右前1番目) |
offset_rf2 | 0 | 関節のオフセット(右前2番目) |
body_length | 0.07 | ボディの長さ |
body_height | 0.07 | ボディの幅 |
body_height | 0.005 | ボディの高さ |
leg0_length | 0.02 | リンク0の長さ |
leg0_height | 0.025 | リンク0の高さ |
leg1_length | 0.055 | リンク1の長さ |
leg2_length | 0.08 | リンク2の長さ |
leg0_offset_x | 0 | リンク0根元のオフセット(X) |
leg0_offset_y | 0 | リンク0根元のオフセット(Y) |
leg0_offset_z | 0 | リンク0根元のオフセット(Z) |
leg1_offset_x | 0 | リンク1根元のオフセット(X) |
leg1_offset_y | 0 | リンク1根元のオフセット(Y) |
leg1_offset_z | 0 | リンク1根元のオフセット(Z) |
leg2_offset_x | 0 | リンク2根元のオフセット(X) |
leg2_offset_y | 0 | リンク2根元のオフセット(Y) |
leg2_offset_z | 0 | リンク2根元のオフセット(Z) |
dir_lf0 | 1 | 関節の回転方向(左前0番目) |
dir_lf1 | 1 | 関節の回転方向(左前1番目) |
dir_lf2 | 1 | 関節の回転方向(左前2番目) |
dir_lb0 | 1 | 関節の回転方向(左後0番目) |
dir_lb1 | 1 | 関節の回転方向(左後1番目) |
dir_lb2 | 1 | 関節の回転方向(左後2番目) |
dir_rb0 | 1 | 関節の回転方向(右後0番目) |
dir_rb1 | 1 | 関節の回転方向(右後1番目) |
dir_rb2 | 1 | 関節の回転方向(右後2番目) |
dir_rf0 | 1 | 関節の回転方向(右前0番目) |
dir_rf1 | 1 | 関節の回転方向(右前1番目) |
dir_rf2 | 1 | 関節の回転方向(右前2番目) |
upper_limit_motor0 | 1.571796 | 関節0の可動範囲上限 |
upper_limit_motor1 | 1.571796 | 関節1の可動範囲上限 |
upper_limit_motor2 | 1.571796 | 関節2の可動範囲上限 |
lower_limit_motor0 | -1.571796 | 関節0の可動範囲下限 |
lower_limit_motor1 | -1.571796 | 関節1の可動範囲下限 |
lower_limit_motor2 | -1.571796 | 関節2の可動範囲下限 |
ArduinoでRCサーボを制御するためのコンポーネントです。 RTnoを利用しています。
FourLeggedRobot_RTno | ||
InPort | ||
名前 | データ型 | 説明 |
in | RTC::TimedDoubleSeq | RCサーボの目標角度 |
EV3でLモーター2個とMモーター1個を利用したマニピュレータを制御するためのコンポーネントです。
FourLeggedRobot_RTno | ||
InPort | ||
名前 | データ型 | 説明 |
in | RTC::TimedDoubleSeq | モーターの目標角度 |
OutPort | ||
名前 | データ型 | 説明 |
out | RTC::TimedDoubleSeq | 現在のモーターの角度 |
コンフィギュレーションパラメータ | ||
名前 | デフォルト値 | 説明 |
motor0_speed | 10.0 | モーター0(Mモーター)の速度 |
motor1_speed | 8.0 | モーター1(Lモーター)の速度 |
motor2_speed | 8.0 | モーター2(Lモーター)の速度 |
motor0_gear_ratio | 2.333 | 関節0のギア比 |
motor1_gear_ratio | 4.0 | 関節1のギア比 |
motor2_gear_ratio | 4.0 | 関節2のギア比 |
motor0_offset | 0 | モーター0のオフセット |
motor1_offset | 0 | モーター1のオフセット |
motor2_offset | 0 | モーター2のオフセット |
Open Dynamics Engineを利用した四足歩行ロボットのシミュレータです。
FourLeggedRobot_RTno | ||
InPort | ||
名前 | データ型 | 説明 |
servo | RTC::TimedDoubleSeq | 関節の目標角度 |
コンフィギュレーションパラメータ | ||
名前 | デフォルト値 | 説明 |
draw_time | 0.01 | 描画の速さ |
sampling_time | -1 | シミュレーションのステップ時間。0以下の場合は実行周期 |
gravity | -9.8 | 重力加速度 |
body_length | 0.07 | ボディの長さ |
body_width | 0.07 | ボディの幅 |
body_height | 0.005 | ボディの高さ |
body_pos_x | 0 | ボディの位置(X) |
body_pos_y | 0 | ボディの位置(Y) |
body_pos_z | 0.115 | ボディの位置(Z) |
body_mass | 0.3 | ボディの重量 |
leg0_length | 0.02 | リンク0の長さ |
leg0_width | 0.02リンク0の幅 | |
leg0_height | 0.025 | リンク0の高さ |
leg1_length | 0.055 | リンク1の長さ |
leg1_width | 0.005 | リンク1の幅 |
leg1_height | 0.01 | リンク1の高さ |
leg2_length | 0.08 | リンク2の長さ |
leg2_width | 0.005 | リンク2の幅 |
leg2_height | 0.005 | リンク2の高さ |
leg0_offset_x | 0 | リンク0根元のオフセット(X) |
leg0_offset_y | 0 | リンク0根元のオフセット(Y) |
leg0_offset_z | 0 | リンク0根元のオフセット(Z) |
leg1_offset_x | 0 | リンク1根元のオフセット(X) |
leg1_offset_y | 0 | リンク1根元のオフセット(Y) |
leg1_offset_z | 0 | リンク1根元のオフセット(Z) |
leg2_offset_x | 0 | リンク2根元のオフセット(X) |
leg2_offset_y | 0 | リンク2根元のオフセット(Y) |
leg2_offset_z | 0 | リンク2根元のオフセット(Z) |
leg3_offset_x | 0 | リンク3根元のオフセット(X) |
leg3_offset_y | 0 | リンク3根元のオフセット(Y) |
leg3_offset_z | 0 | リンク3根元のオフセット(Z) |
leg0_mass | 0.2 | リンク0の重量 |
leg1_mass | 0.1 | リンク1の重量 |
leg2_mass | 0.001 | リンク2の重量 |
foot_length | 0.16 | 足裏の長さ |
foot_width | 0.12 | 足裏の幅 |
foot_height0.005 | 足裏の高さ | |
foot_mass | 0.1 | 足裏の重量 |
foot_exist | 0 | 足裏の有無 |
インターフェースの説明の前に、多足歩行ロボットのRTCの設計方針について述べる。 まず、対象とする多足歩行ロボットは自律移動機能共通インタフェースに準拠し、起動追従RTC等から目標速度を入力可能とする。
四足歩行ロボットの歩容だけでもクロール歩容、間歇クロール歩容、トロット歩容等様々な種類があり、ロボットの機構もさまざまであることから、四足歩行ロボット制御RTCを適当な粒度に分割する。
脚運動生成系に関しては四足歩行ロボットの機構への依存が小さく,脚の可動範囲が同じであれば関節の配置や自由度が違っていても歩行は可能である. 足先軌道制御系に関しては四足歩行ロボットの機構に大きく依存する部分であり,脚のリンク長さ,関節位置等詳細なパラメータが既知である必要がある. 駆動サーボ制御系に関しては四足歩行ロボットのハードウェアに直接依存する部分であるが,使用する駆動サーボが同じ場合は四足歩行ロボット以外にも適用可能である.
よって,機構への依存が小さい部分,機構への依存が大きい部分,ハードウェアに依存する部分の3 種類に分割できたため,以下の3 種類の処理を行うRTCを作成しシステムを作成する.
足先軌道制御コンポーネントと駆動サーボ制御コンポーネントの間の通信には,駆動サーボ制御コンポーネントが既存のRTC を利用できる可能性があることからデータポートによりTimedDoubleSeq 型等の基本データ型を利用した通信を行うべきである.
脚運動生成コンポーネントと足先軌道制御コンポーネントの間の通信には既存のインターフェースで適当なものが存在しないため、独自のデータ型を定義する。
多足歩行ロボット制御用共通インターフェースとして、以下のIDLファイルを作成した。
// -*- IDL -*- #ifndef LEGGEDROBOTINTERFACE_IDL #define LEGGEDROBOTINTERFACE_IDL #include "BasicDataType.idl" #include "ExtendedDataTypes.idl" module RTC { struct Leg { RTC::Pose3D pose; RTC::Velocity3D velocity; boolean onGround; }; struct LeggedRobot { RTC::Time tm; sequence<Leg> leg_data; RTC::Velocity3D body_velocity; RTC::Pose3D body_pose; }; }; /* module */ #endif /* LEGGEDROBOTINTERFACE_IDL */
この共通インターフェースでは以下の図の座標系を使用する。 Σwは床に固定された座標系(ワールド座標系)、Σrはロボットの重心を原点とした座標系(ロボット重心座標系)とする。
新たに定義したデータ型LeggedRobot型は以下の足先軌道の通信に使用する。 LeggedRobot型はLeg型の配列を保持しており、各脚の目標位置、目標速度をLeg型オブジェクトに格納する。 四足歩行ロボットの場合は、以下のように脚番号を設定してある。
配列leg_dataには脚0から順に格納する。
Leg型、LeggedRobot型の各変数の内容は以下のとおりである。
Leg型 | ||
変数名 | 型 | 意味 |
pose | RTC::Pose3D | 目標足先位置(重心座標系) |
velocity | RTC::Velocity3D | 目標足先速度(重心座標系) |
onGround | boolean | 地面に接地する場合はtrue,接地しない場合はfalse |
LeggedRobot型 | ||
変数名 | 型 | 意味 |
tm | RTC::Time | タイムスタンプ |
leg_data | sequence<Leg> | 各脚の足先軌道 |
body_velocity | RTC::Velocity3D | 重心の目標速度(ワールド座標系) |
body_pose | RTC::Pose3D | 重心位置(ワールド座標系) |
このページでは以下の4種類のRTシステムにより動作確認を行う手順を説明します。
クロール歩容により四足歩行ロボット実機を歩行させるRTシステムです。 四足歩行ロボットの操作はOpenRTM-aist Python版付属のジョイスティックコンポーネントで行います。
クロール歩容によりシミュレータ上の四足歩行ロボットを歩行させるRTシステムです。
間歇クロール歩容により四足歩行ロボット実機を歩行させるRTシステムです。
間歇クロール歩容によりシミュレータ上の四足歩行ロボットを歩行させるRTシステムです。
トロット歩容により四足歩行ロボット実機を歩行させるRTシステムです。
トロット歩容によりシミュレータ上の四足歩行ロボットを歩行させるRTシステムです。
以下の手順で動作確認を行います。
SunFounder製四足歩行ロボットを説明書通りに組み立ててください。 ネジが固くて回りにくいのですが、ネジ穴が潰れる覚悟で頑張ってください。
PCとSunFounder NanoをUSBケーブルで接続してください。 接続に成功するとSunFounder NanoのLEDが点灯します。
少し古いですが、RTno利用のためにArduino IDE 1.06をインストールしてください。
既に新しいバージョンをインストールしている場合はZIPファイルをダウンロードして適当な場所に展開してください。 インストーラーは「Windows Installer 」、ZIPファイルは「Windows ZIP file for non admin install」をクリックして移動したページで「JUST DOWNLOAD」をクリックするとダウンロードできます。
RTnoはArduinoとRTCの通信を簡単にするためのライブラリです。
以下のリンクからダウンロードしたZIPファイルを解凍してフォルダ名をRTnoに変更後、Arduino IDEをインストールしたフォルダのlibrariesフォルダにコピーします。
以下のリンクからモーター制御用のソースコードをダウンロードしてください。
上記ファイルを適当な場所に展開して、FourLeggedRobot.inoをArduino IDEで開いてください。 インストーラーでArduino IDEをインストールした場合はダブルクリックしたら開けますが、ZIPファイルを展開した場合はarduino.exeにFourLeggedRobot.inoをドラッグアンドドロップしてください。
Arduino IDEが起動したら、ツール→シリアルポートからSunFounder Nanoを接続したポート名を選択してください。
以下のボタンをクリックしてエラーが表示されなければ、SunFounder Nanoへのプログラムの書き込みは完了です。
※事前にネームサーバーを起動してください。
以下のファイルをダウンロードして、適当な場所に展開してください。
※このスクリプトファイル実行には以下のソフトウェアのインストールが必要です。
OpenRTM-aist-1.1.2の場合、デフォルトでインストールされますが、1.1.1以前を使用している場合は個別にインストールしてください。
展開したフォルダの「Components」→「CPP」→「RTnoProxy.conf」をメモ帳などで開いてください。 以下の部分をArduino IDEでプログラムを書きこんだポート名に変更して保存してください。
conf.default.comport:\\\\.\\COM5
編集が完了したら、start_component.batをダブルクリックしてください。 これで以下のRTCが起動します。
起動したかどうかをRTシステムエディタで確認してください。
実機をクロール歩容により歩行させて動作確認を行います。
RTCが起動したら、「CrawlGaitRobot」フォルダの「resurrect_CrawlGaitRobot.bat」を起動してください。 これでデータポートが以下のように接続されます。
接続するポート | |||
アウトポート | インポート | ||
モジュール名 | ポート名 | モジュール名 | ポート名 |
TkJoyStickComp0 | pos | FloatSeqToVelocity0 | in |
FloatSeqToVelocity0 | out | Crawl_Gait_Controller0 | target_velocity |
Crawl_Gait_Controller0 | Trajectory | Foot_Position_Controller0 | Trajectory |
Foot_Position_Controller0 | motor_pos | RTnoProxy0 | in |
start_CrawlGaitRobot.batをダブルクリックすると、各RTCをアクティブ化してジョイスティックによる四足歩行ロボットの操作ができるようになります。
非アクティブ化をする場合はstop_CrawlGaitRobot.bat、ポートを切断する場合はteardown_CrawlGaitRobot.batを起動してください。
スクリプトファイル名 | 動作内容 |
resurrect_CrawlGaitRobot.bat | RTシステムの復元 |
start_CrawlGaitRobot.bat | RTCのアクティブ化 |
stop_CrawlGaitRobot.bat | RTCの非アクティブ化 |
teardown_CrawlGaitRobot.bat | ポート切断 |
実機がない場合はシミュレータで動作確認をすることもできます。 RTCが起動していない場合はstart_component.batをダブルクリックして起動してください。 「CrawlGaitSimulation」フォルダの「resurrect_CrawlGaitSimulation.bat」を起動してください。 すると以下のように接続されます。
接続するポート | |||
アウトポート | インポート | ||
モジュール名 | ポート名 | モジュール名 | ポート名 |
TkJoyStickComp0 | pos | FloatSeqToVelocity0 | in |
FloatSeqToVelocity0 | out | Crawl_Gait_Controller0 | target_velocity |
Crawl_Gait_Controller0 | Trajectory | Foot_Position_Controller0 | Trajectory |
Foot_Position_Controller0 | motor_pos | Four_legged_Robot_Simulator0 | servo |
start_CrawlGaitSimulation.batを起動するとRTCをアクティブ化し、ジョイスティックでシミュレータ上の四足歩行ロボットの操作ができるようになります。
非アクティブ化をする場合はstop_CrawlGaitSimulation.bat、ポートを切断する場合はteardown_CrawlGaitSimulation.batを起動してください。
スクリプトファイル名 | 動作内容 |
resurrect_CrawlGaitSimulation.bat | RTシステムの復元 |
start_CrawlGaitSimulation.bat | RTCのアクティブ化 |
stop_CrawlGaitSimulation.bat | RTCの非アクティブ化 |
teardown_CrawlGaitSimulation.bat | ポート切断 |
実機を間歇クロール歩容により歩行させて動作確認を行います。
RTCが起動していない場合はstart_component.batをダブルクリックして起動してください。 RTCが起動したら、「IntermittentCrawlGaitRobot」フォルダの「resurrect_IntermittentCrawlGaitRobot.bat」を起動してください。 これでデータポートが以下のように接続されます。
接続するポート | |||
アウトポート | インポート | ||
モジュール名 | ポート名 | モジュール名 | ポート名 |
TkJoyStickComp0 | pos | FloatSeqToVelocity0 | in |
FloatSeqToVelocity0 | out | Intermittent_Crawl_Gait_Controller0 | target_velocity |
Intermittent_Crawl_Gait_Controller0 | Trajectory | Foot_Position_Controller0 | Trajectory |
Foot_Position_Controller0 | motor_pos | RTnoProxy0 | in |
start_IntermittentCrawlGaitRobot.batを起動するとRTCをアクティブ化し、ジョイスティックで四足歩行ロボットの操作ができるようになります。
非アクティブ化をする場合はstop_IntermittentCrawlGaitRobot.bat、ポートを切断する場合はteardown_IntermittentCrawlGaitRobot.batを起動してください。
スクリプトファイル名 | 動作内容 |
resurrect_IntermittentCrawlGaitRobot.bat | RTシステムの復元 |
start_IntermittentCrawlGaitRobot.bat | RTCのアクティブ化 |
stop_IntermittentCrawlGaitRobot.bat | RTCの非アクティブ化 |
teardown_IntermittentCrawlGaitRobot.bat | ポート切断 |
シミュレータ動作確認をすることもできます。 RTCが起動していない場合はstart_component.batをダブルクリックして起動してください。 「IntermittentCrawlGaitSimulation」フォルダの「resurrect_IntermittentCrawlGaitSimulation.bat」を起動してください。 すると以下のように接続されます。
接続するポート | |||
アウトポート | インポート | ||
モジュール名 | ポート名 | モジュール名 | ポート名 |
TkJoyStickComp0 | pos | FloatSeqToVelocity0 | in |
FloatSeqToVelocity0 | out | Intermittent_Crawl_Gait_Controller0 | target_velocity |
Intermittent_Crawl_Gait_Controller0 | Trajectory | Foot_Position_Controller0 | Trajectory |
Foot_Position_Controller0 | motor_pos | Four_legged_Robot_Simulator0 | servo |
start_IntermittentCrawlGaitSimulation.batを起動するとRTCをアクティブ化し、ジョイスティックでシミュレータ上の四足歩行ロボットの操作ができるようになります。
非アクティブ化をする場合はstop_IntermittentCrawlGaitSimulation.bat、ポートを切断する場合はteardown_IntermittentCrawlGaitSimulation.batを起動してください。
スクリプトファイル名 | 動作内容 |
resurrect_IntermittentCrawlGaitSimulation.bat | RTシステムの復元 |
start_IntermittentCrawlGaitSimulation.bat | RTCのアクティブ化 |
stop_IntermittentCrawlGaitSimulation.bat | RTCの非アクティブ化 |
teardown_IntermittentCrawlGaitSimulation.bat | ポート切断 |
実機をトロット歩容により歩行させて動作確認を行います。
RTCが起動していない場合はstart_component.batをダブルクリックして起動してください。 RTCが起動したら、「TrotGaitRobot」フォルダの「resurrect_TrotGaitRobot.bat」を起動してください。 これでデータポートが以下のように接続されます。
接続するポート | |||
アウトポート | インポート | ||
モジュール名 | ポート名 | モジュール名 | ポート名 |
TkJoyStickComp0 | pos | FloatSeqToVelocity0 | in |
FloatSeqToVelocity0 | out | Trot_Gait_Controller0 | target_velocity |
Trot_Gait_Controller0 | Trajectory | Foot_Position_Controller0 | Trajectory |
Foot_Position_Controller0 | motor_pos | RTnoProxy0 | in |
start_TrotGaitRobot.batを起動するとRTCをアクティブ化し、ジョイスティックで四足歩行ロボットの操作ができるようになります。
非アクティブ化をする場合はstop_TrotGaitRobot.bat、ポートを切断する場合はteardown_TrotGaitRobot.batを起動してください。
スクリプトファイル名 | 動作内容 |
resurrect_TrotGaitRobot.bat | RTシステムの復元 |
start_TrotGaitRobot.bat | RTCのアクティブ化 |
stop_TrotGaitRobot.bat | RTCの非アクティブ化 |
teardown_TrotGaitRobot.bat | ポート切断 |
シミュレータ動作確認をすることもできます。 RTCが起動していない場合はstart_component.batをダブルクリックして起動してください。 「TrotGaitSimulation」フォルダの「resurrect_TrotGaitSimulation.bat」を起動してください。 すると以下のように接続されます。
接続するポート | |||
アウトポート | インポート | ||
モジュール名 | ポート名 | モジュール名 | ポート名 |
TkJoyStickComp0 | pos | FloatSeqToVelocity0 | in |
FloatSeqToVelocity0 | out | Trot_Gait_Controller0 | target_velocity |
Trot_Gait_Controller0 | Trajectory | Foot_Position_Controller0 | Trajectory |
Foot_Position_Controller0 | motor_pos | Four_legged_Robot_Simulator0 | servo |
start_TrotGaitSimulation.batを起動するとRTCをアクティブ化し、ジョイスティックでシミュレータ上の四足歩行ロボットの操作ができるようになります。
非アクティブ化をする場合はstop_TrotGaitSimulation.bat、ポートを切断する場合はteardown_TrotGaitSimulation.batを起動してください。
スクリプトファイル名 | 動作内容 |
resurrect_TrotGaitSimulation.bat | RTシステムの復元 |
start_TrotGaitSimulation.bat | RTCのアクティブ化 |
stop_TrotGaitSimulation.bat | RTCの非アクティブ化 |
teardown_TrotGaitSimulation.bat | ポート切断 |
このページではLEGO Mindsorms EV3により組み立てた四足歩行ロボットを制御する手順について説明します。
以下の手順で動作確認を行います。
まずは教育版レゴ マインドストームEV3基本セットを4台用意してください。 ない場合は友達から借りてください。
脚の外観は以下のようになっています。 頑張って組み立ててください。
画像を見てもらえばわかる通り、Mモーター1個とLモーター2個を使用しています。 以降、根元からMモーター0、Lモーター1、Lモーター2とします。
以下のギアで減速しています。 直径1cmのギアをモーター側、直径4cmのギアを関節側に取り付けてください。
足裏は以下のように平行リンク機構により常に胴体と平行になるようにしています。
胴体部分は以下のようになっています。
次にEV3の初期設定を行い、EV3を無線LANに接続してください。
EV3のホスト名を変更します。 どの脚を制御するEV3かによって、ホスト名をev3-legged-robot0~ev3-legged-robot3と設定します。
まずは/etc/hostnameを開いてev3-legged-robot0(脚番号で末尾の数字は変更)を入力してください。
sudo nano /etc/hostname
以下のコマンドで/etc/hostsを編集してください。
sudo nano /etc/hosts
127.0.0.1 ev3-legged-robot0 ev3-legged-robot0.local
編集が完了したら再起動してください。
OpenRTM-aistを以下の手順でインストールしてください。
脚制御コンポーネントEV3_Leg_Controllerをインストールします。
以下のコマンドを入力してください。
git clone https://github.com/Nobu19800/EV3_Leg_Controller.git cd EV3_Leg_Controller mkdir build cd build cmake .. make
これで「EV3_Leg_Controller」→「build」→「src」フォルダ内に実行ファイルEV3_Leg_ControllerCompが生成されます。
次に以下のようにEV3とモーターを接続してください。
ポートA | Mモーター0 |
ポートB | Lモーター1 |
ポートC | Lモーター2 |
最後にEV3_Leg_Controllerを起動して準備完了です。
./EV3_Leg_Controller/build/src/EV3_Leg_ControllerComp
ここからはPC上で操作を行います。 事前にネームサーバーを起動しておいてください。
まずジョイスティックコンポーネント、脚運動生成コンポーネント、足先軌道制御コンポーネントの起動を行います。 以下のファイルをダウンロードして適当な場所に展開してください。
展開したらstart_component.batをダブルクリックしてください。 これで以下のRTCが起動します。
起動したかどうかをRTシステムエディタで確認してください。
間歇クロール歩容により歩行させて動作確認を行います。 RTCが起動したら、「IntermittentCrawlGaitEV3」フォルダの「resurrect_IntermittentCrawlGaitEV3.bat」を起動してください。 これでデータポートが以下のように接続されます。
接続するポート | |||
アウトポート | インポート | ||
モジュール名 | ポート名 | モジュール名 | ポート名 |
TkJoyStickComp0 | pos | FloatSeqToVelocity0 | in |
FloatSeqToVelocity0 | out | Intermittent_Crawl_Gait_Controller0 | target_velocity |
Intermittent_Crawl_Gait_Controller0 | Trajectory | Foot_Position_Controller0 | Trajectory |
Foot_Position_Controller0 | motor_pos_0 | EV3_Leg_Controller0(ev3-legged-robot0) | in |
Foot_Position_Controller0 | motor_pos_1 | EV3_Leg_Controller0(ev3-legged-robot1) | in |
Foot_Position_Controller0 | motor_pos_2 | EV3_Leg_Controller0(ev3-legged-robot2) | in |
Foot_Position_Controller0 | motor_pos_3 | EV3_Leg_Controller0(ev3-legged-robot3) | in |
start_IntermittentCrawlGaitEV3.batを起動するとRTCをアクティブ化し、ジョイスティックで四足歩行ロボットの操作ができるようになります。
非アクティブ化をする場合はstop_IntermittentCrawlGaitEV3.bat、ポートを切断する場合はteardown_IntermittentCrawlGaitEV3.batを起動してください。
各歩容については以下の資料に一通り情報は載っているようなので、ここでは最低限だけ説明します。
低速な歩行であり、常に3脚以上接地してあるため静的に安定な歩行が可能です。 右前脚→左後脚→左前脚→右前脚の順番で遊脚を行います。 画像の赤色の多角形は接地脚を結んだ多角形(支持脚多角形)、青色の円はロボットの重心位置を地面に投影した点から多角形の3辺までの距離の最小値(重心位置から最も近い辺までの距離)を表しています。 この円の半径を安定余裕といいます。
画像を見てもらえばわかる通り、安定余裕が0になる期間があります。 この間は非常に不安定になります。
安定余裕を0より大きくするためには安定余裕が0になるタイミングで四脚支持に遷移する必要があります。
横移動により支持脚多角形の内部に重心投影点を移動させることでより安定な歩行が可能です。 その反面、加減速が大きい等のデメリットはあります。